forked from a1batross/Paranoia2_original
89 lines
2.4 KiB
C++
89 lines
2.4 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
|