Matrix
LookAt(float eyex, float eyey, float eyez,
float centerx, float centery, float centerz,
float upx, float upy, float upz)
{
Vector3 z(eyex - centerx, eyey - centery, eyez - centerz);
Vector3Normalize(z, z);
Vector3 x(
upy * z[2] - upz * z[1],
-upx * z[2] + upz * z[0],
upx * z[1] - upy * z[0]);
Vector3 y(
z[1] * x[2] - z[2] * x[1],
-z[0] * x[2] + z[2] * x[0],
z[0] * x[1] - z[1] * x[0]);
Vector3Normalize(x, x);
Vector3Normalize(y, y);
Matrix mat(
x[0], y[0], z[0], 0,
x[1], y[1], z[1], 0,
x[2], y[2], z[2], 0,
0, 0, 0, 1);
Matrix mat2;
MatrixTranslation(mat2, -eyex, -eyey, -eyez);
Matrix ret;
MatrixMultiply(ret, mat, mat2);
return ret;
}
static inline Math::Vector3<Type> Vector3Normalv(const Math::Vector3<Type> &u,
const Math::Vector3<Type> &v)
{
Math::Vector3<Type> U = Vector3Normalize(1e-7, u);
Math::Vector3<Type> V = Vector3Normalize(1e-7, v);
Math::Vector3<Type> N = U ^ V;
return Vector3Normalize(1e-7, N);
} // Vector3Normalv
//==========================
// ボーンを指定座標へ向ける
//==========================
void cPMDBone::lookAt( const Vector3 *pvecTargetPos, float fLimitXD, float fLimitXU, float fLimitY )
{
// どうもおかしいので要調整
Matrix matTemp;
MatrixIdentity( matTemp );
matTemp[3][0] = m_vec3Position.x + m_vec3Offset.x;
matTemp[3][1] = m_vec3Position.y + m_vec3Offset.y;
matTemp[3][2] = m_vec3Position.z + m_vec3Offset.z;
if( m_pParentBone )
{
Matrix matInvTemp;
MatrixInverse( matInvTemp, m_pParentBone->m_matLocal );
matInvTemp[3][0] = m_pParentBone->m_matLocal[3][0];
matInvTemp[3][1] = m_pParentBone->m_matLocal[3][1];
matInvTemp[3][2] = -m_pParentBone->m_matLocal[3][2];
MatrixMultiply( matTemp, matTemp, matInvTemp );
}
MatrixInverse( matTemp, matTemp );
Vector3 vec3LocalTgtPosZY;
Vector3 vec3LocalTgtPosXZ;
Vector3Transform( &vec3LocalTgtPosZY, pvecTargetPos, matTemp );
vec3LocalTgtPosXZ = vec3LocalTgtPosZY;
vec3LocalTgtPosXZ.y = 0.0f;
Vector3Normalize( &vec3LocalTgtPosXZ, &vec3LocalTgtPosXZ );
vec3LocalTgtPosZY.x = 0.0f;
Vector3Normalize( &vec3LocalTgtPosZY, &vec3LocalTgtPosZY );
Vector3 vec3Angle = { 0.0f, 0.0f, 0.0f };
vec3Angle.x = asinf( vec3LocalTgtPosZY.y );
if( vec3LocalTgtPosXZ.x < 0.0f ) vec3Angle.y = acosf( vec3LocalTgtPosXZ.z );
else vec3Angle.y = -acosf( vec3LocalTgtPosXZ.z );
if( vec3Angle.x < RAD(fLimitXD) ) vec3Angle.x = RAD(fLimitXD);
if( RAD(fLimitXU) < vec3Angle.x ) vec3Angle.x = RAD(fLimitXU);
if( vec3Angle.y < RAD(-fLimitY) ) vec3Angle.y = RAD(-fLimitY);
if( RAD(fLimitY) < vec3Angle.y ) vec3Angle.y = RAD( fLimitY);
Vector4 vec4RotTemp;
QuaternionCreateEuler( &vec4RotTemp, &vec3Angle );
QuaternionSlerp( &m_vec4LookRotation, &m_vec4LookRotation, &vec4RotTemp, 0.5f ); // 0.3f ); // 視線の動きを高速化
m_vec4Rotation = m_vec4LookRotation;
}
static inline Math::Vector3<Type> Vector3Normalv(const Math::Vector3<Type> &u,
const Math::Vector3<Type> &v,
const Math::Vector3<Type> &w)
{
Math::Vector3<Type> dv = v - u;
Math::Vector3<Type> V = Vector3Normalize(1e-7, dv);
Math::Vector3<Type> dw = w - u;
Math::Vector3<Type> W = Vector3Normalize(1e-7, dw);
Math::Vector3<Type> N = V ^ W;
return Vector3Normalize(1e-7, N);
} // Vector3Normal
internal Vector3 seek_return(ActorSteerData *actor, Vector3 target) {
Vector3 desired = Vector3Subtract(target, actor->location);
if (desired.x != 0 || desired.y != 0 || desired.z != 0) {
desired = Vector3Normalize(desired);
}
desired = Vector3MultiplyScalar(desired, actor->max_speed);
Vector3 steer = Vector3Subtract(desired, actor->velocity);
steer = Vector3Limit(steer, actor->max_force);
return steer;
}
//======
// 更新
//======
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();
}
}
}
}
Math::Vector3<float> Math::norml(const float &e,
const Math::Vector3<float> &v)
{
return Vector3Normalize(e, v);
} // norml
Math::Vector3<float> Math::norml(const Math::Vector3<float> &v)
{
return Vector3Normalize(1e-7, v);
} // norml
Math::Vector3<double> Math::norml(const double &e,
const Math::Vector3<double> &v)
{
return Vector3Normalize(e, v);
} // norml
Math::Vector3<double> Math::norml(const Math::Vector3<double> &v)
{
return Vector3Normalize(1e-7, v);
} // norml
