void test_basic_quaternion_multiply(void) {
Quaternion q = { 1, 0, 0, 0 };
Quaternion r = { 1, 0, 0, 0 };
Quaternion t;
Vector3F eulers;
QuaternionMultiply(&q, &r, &t);
QuaternionPrint(&t);
assert(t.a == 1);
assert(t.b == 0);
assert(t.c == 0);
assert(t.d == 0);
q.a = 0.707107;
q.b = 0.707107;
QuaternionMultiply(&q, &r, &t);
QuaternionPrint(&t);
QuaternionToEulers(&t, &eulers);
assert(NearEqual(eulers.a, PI_DIV_2, 0.001));
assert(eulers.b == 0);
assert(eulers.c == 0);
QuaternionMultiply(&q, &q, &t);
QuaternionPrint(&t);
QuaternionToEulers(&t, &eulers);
assert(NearEqual(eulers.a, PI, 0.001));
assert(eulers.b == 0);
assert(eulers.c == 0);
}
int CheckAnswerA (double *q)
{
double newq1[4], newq2[4], stepq[4], min_f, temp1, temp2;
FN_A (q, &min_f);
stepq[0] = M_SQRT1_2; stepq[1] = 0.0; stepq[2] = M_SQRT1_2; stepq[3] = 0.0;
QuaternionMultiply (newq1, q, stepq);
FN_A (newq1, &temp1);
stepq[0] = M_SQRT1_2; stepq[1] = M_SQRT1_2; stepq[2] = 0.0; stepq[3] = 0.0;
QuaternionMultiply (newq2, q, stepq);
FN_A (newq2, &temp2);
if ((temp1 < min_f) && (temp1 <= temp2)) {
q[0] = newq1[0]; q[1] = newq1[1]; q[2] = newq1[2]; q[3] = newq1[3];
}
if ((temp2 < min_f) && (temp2 <= temp1)) {
q[0] = newq2[0]; q[1] = newq2[1]; q[2] = newq2[2]; q[3] = newq2[3];
}
return 0;
}
/**
* クォータニオンの乗算代入
*/
Quaternion& Quaternion::operator *= (const Quaternion& p)
{
Quaternion _q;
QuaternionMultiply(_q, *this, p);
*this = _q;
return *this;
}
int AddCameraPositionNoise (position *ThePos, double r_error, double t_error)
{
int idum = 1;
double stepq[4], newq[4];
/* select a random rotation */
RandomQuaternion (stepq, r_error);
QuaternionMultiply (newq, stepq, ThePos->q);
ThePos->q[0] = newq[0];
ThePos->q[1] = newq[1];
ThePos->q[2] = newq[2];
ThePos->q[3] = newq[3];
ThePos->t[0] += t_error * ran1;
ThePos->t[1] += t_error * ran1;
ThePos->t[2] += t_error * ran1;
return 0;
}
static float * CorrectQuaternionWithAccelerometer(float quat[4])
{
// Assume that the accelerometer measures ONLY the resistance to gravity
// (opposite the gravity vector). The direction of rotation that takes the
// body from predicted to estimated gravity is (-accelerometer x g_b_ x). This
// is equivalent to (g_b_ x accelerometer). Form a corrective quaternion from
// this rotation.
float quat_c[4] = { 1.0, 0.0, 0.0, 0.0 };
VectorCross(g_b_, AccelerationVector(), &quat_c[1]);
quat_c[1] *= 0.5 * ACCELEROMETER_CORRECTION_GAIN;
quat_c[2] *= 0.5 * ACCELEROMETER_CORRECTION_GAIN;
quat_c[3] *= 0.5 * ACCELEROMETER_CORRECTION_GAIN;
// Apply the correction to the attitude quaternion.
float result[4];
QuaternionMultiply(quat, quat_c, result);
quat[0] = result[0];
quat[1] = result[1];
quat[2] = result[2];
quat[3] = result[3];
return quat;
}
/**
* クォータニオンの乗算
*/
Quaternion Quaternion::operator * (const Quaternion& p)
{
Quaternion _q;
QuaternionMultiply(_q, *this, p);
return _q;
}
Vector3D QuaternionMultiplyVector(Quaternion quaternion, Vector3D vector) {
Quaternion vectorQuaternion = Vector3DGetQuaternion(vector);
Quaternion invertQuaternion = QuaternionInvert(quaternion);
return QuaternionGetVector3D(QuaternionMultiply(vectorQuaternion, invertQuaternion));
}
Quaternion QuaternionRotate(Quaternion quaternion, Vector3D axis, float angle) {
return QuaternionMultiply(quaternion, QuaternionInitAxisAngle(axis, angle));
}
//======
// 更新
//======
void cPMDIK::update( void )
{
Vector3 vec3OrgTargetPos;
vec3OrgTargetPos.x = m_pTargetBone->m_matLocal[3][0];
vec3OrgTargetPos.y = m_pTargetBone->m_matLocal[3][1];
vec3OrgTargetPos.z = m_pTargetBone->m_matLocal[3][2];
Vector3 vec3EffPos;
Vector3 vec3TargetPos;
for( short i = m_cbNumLink - 1 ; i >= 0 ; i-- ){ m_ppBoneList[i]->updateMatrix(); }
m_pEffBone->updateMatrix();
for( unsigned short it = 0 ; it < m_unCount ; it++ )
{
for( unsigned char cbLinkIdx = 0 ; cbLinkIdx < m_cbNumLink ; cbLinkIdx++ )
{
// エフェクタの位置の取得
vec3EffPos.x = m_pEffBone->m_matLocal[3][0];
vec3EffPos.y = m_pEffBone->m_matLocal[3][1];
vec3EffPos.z = m_pEffBone->m_matLocal[3][2];
// ワールド座標系から注目ノードの局所(ローカル)座標系への変換
Matrix matInvBone;
MatrixInverse( matInvBone, m_ppBoneList[cbLinkIdx]->m_matLocal );
// エフェクタ，到達目標のローカル位置
Vector3Transform( &vec3EffPos, &vec3EffPos, matInvBone );
Vector3Transform( &vec3TargetPos, &vec3OrgTargetPos, matInvBone );
// 十分近ければ終了
Vector3 vec3Diff;
Vector3Sub( &vec3Diff, &vec3EffPos, &vec3TargetPos );
if( Vector3DotProduct( &vec3Diff, &vec3Diff ) < 0.0000001f ) return;
// (1) 基準関節→エフェクタ位置への方向ベクトル
Vector3Normalize( &vec3EffPos, &vec3EffPos );
// (2) 基準関節→目標位置への方向ベクトル
Vector3Normalize( &vec3TargetPos, &vec3TargetPos );
// ベクトル (1) を (2) に一致させるための最短回転量（Axis-Angle）
//
// 回転角
float fRotAngle = acosf( Vector3DotProduct( &vec3EffPos, &vec3TargetPos ) );
if( 0.00000001f < fabsf( fRotAngle ) )
{
if( fRotAngle < -m_fFact ) fRotAngle = -m_fFact;
else if( m_fFact < fRotAngle ) fRotAngle = m_fFact;
// 回転軸
Vector3 vec3RotAxis;
Vector3CrossProduct( &vec3RotAxis, &vec3EffPos, &vec3TargetPos );
if( Vector3DotProduct( &vec3RotAxis, &vec3RotAxis ) < 0.0000001f ) continue;
Vector3Normalize( &vec3RotAxis, &vec3RotAxis );
// 関節回転量の補正
Vector4 vec4RotQuat;
QuaternionCreateAxis( &vec4RotQuat, &vec3RotAxis, fRotAngle );
if( m_ppBoneList[cbLinkIdx]->m_bIKLimitAngle ) limitAngle( &vec4RotQuat, &vec4RotQuat );
QuaternionNormalize( &vec4RotQuat, &vec4RotQuat );
QuaternionMultiply( &m_ppBoneList[cbLinkIdx]->m_vec4Rotation, &m_ppBoneList[cbLinkIdx]->m_vec4Rotation, &vec4RotQuat );
QuaternionNormalize( &m_ppBoneList[cbLinkIdx]->m_vec4Rotation, &m_ppBoneList[cbLinkIdx]->m_vec4Rotation );
for( short i = cbLinkIdx ; i >= 0 ; i-- ){ m_ppBoneList[i]->updateMatrix(); }
m_pEffBone->updateMatrix();
}
}
}
}
static void transform(float *point, vect4_t q){
vect4_t qin, qout;
VectorCopy(point, qin);
QuaternionMultiply(qin, q, qout);
VectorCopy(qout, point);
}
