89 lines
2.5 KiB
C++
89 lines
2.5 KiB
C++
/*
|
||
iksolver.h - Ken Perlin' inverse kinematic solver
|
||
Copyright (C) 2017 Uncle Mike
|
||
|
||
This program is free software: you can redistribute it and/or modify
|
||
it under the terms of the GNU General Public License as published by
|
||
the Free Software Foundation, either version 3 of the License, or
|
||
(at your option) any later version.
|
||
|
||
This program is distributed in the hope that it will be useful,
|
||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
GNU General Public License for more details.
|
||
*/
|
||
|
||
#ifndef IKSOLVER_H
|
||
#define IKSOLVER_H
|
||
|
||
class CIKSolver
|
||
{
|
||
public:
|
||
//------------ GENERAL VECTOR MATH SUPPORT -----------
|
||
static float dot( float const a[], float const b[] ) { return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; }
|
||
static float length( float const v[] ) { return sqrt( dot( v, v )); }
|
||
static void normalize( float v[] ) { float norm = length( v ); v[0] /= norm; v[1] /= norm; v[2] /= norm; }
|
||
static float findD( float a, float b, float c ) { return (c + (a * a - b * b) / c) / 2; }
|
||
static float findE( float a, float d ) { return sqrt(a * a - d * d); }
|
||
|
||
static void cross( float const a[], float const b[], float c[] )
|
||
{
|
||
c[0] = a[1] * b[2] - a[2] * b[1];
|
||
c[1] = a[2] * b[0] - a[0] * b[2];
|
||
c[2] = a[0] * b[1] - a[1] * b[0];
|
||
}
|
||
|
||
static void rot( float const M[3][3], float const src[], float dst[] )
|
||
{
|
||
dst[0] = dot( M[0], src );
|
||
dst[1] = dot( M[1], src );
|
||
dst[2] = dot( M[2], src );
|
||
}
|
||
|
||
void defineM( float const P[], float const D[] )
|
||
{
|
||
float *X = Minv[0], *Y = Minv[1], *Z = Minv[2];
|
||
int i;
|
||
|
||
for( i = 0; i < 3; i++ )
|
||
X[i] = P[i];
|
||
normalize( X );
|
||
|
||
// Its y axis is perpendicular to P, so Y = unit( E - X(E·X) ).
|
||
float dDOTx = dot( D, X );
|
||
|
||
for( i = 0; i < 3; i++ )
|
||
Y[i] = D[i] - dDOTx * X[i];
|
||
normalize( Y );
|
||
|
||
// Its z axis is perpendicular to both X and Y, so Z = X×Y.
|
||
cross( X, Y, Z );
|
||
|
||
// Mfwd = (Minv)T, since transposing inverts a rotation matrix.
|
||
for( i = 0; i < 3; i++ )
|
||
{
|
||
Mfwd[i][0] = Minv[0][i];
|
||
Mfwd[i][1] = Minv[1][i];
|
||
Mfwd[i][2] = Minv[2][i];
|
||
}
|
||
}
|
||
|
||
bool solve( float A, float B, float const P[], float const D[], float Q[] )
|
||
{
|
||
float R[3];
|
||
|
||
defineM( P, D );
|
||
rot( Minv, P, R );
|
||
float r = length( R );
|
||
float d = findD( A, B, r );
|
||
float e = findE( A, d );
|
||
float S[3] = { d, e, 0 };
|
||
rot( Mfwd, S, Q );
|
||
return d > (r - B) && d < A;
|
||
}
|
||
|
||
float Mfwd[3][3];
|
||
float Minv[3][3];
|
||
};
|
||
|
||
#endif//IKSOLVER_H
|