double cMorph::AkimaInterpolate(const double factor, double v1, double v2, double v3, double v4,
double v5, double v6, const bool angular)
{
if (angular)
{
QList<double*> vals;
vals << &v1 << &v2 << &v3 << &v4 << &v5 << &v6;
NearestNeighbourAngle(vals);
}
double x[] = { -2, -1, 0, 1, 2, 3 };
double y[] = { v1, v2, v3, v4, v5, v6 };
// more info: http://www.alglib.net/interpolation/spline3.php
gsl_spline_init(splineAkimaPeriodic, x, y, listSize);
double value = gsl_spline_eval(splineAkimaPeriodic, factor, interpolationAccelerator);
if (angular)
{
return LimitAngle(value);
}
else
{
return value;
}
}
double cMorph::CatmullRomInterpolate(const double factor, double v1, double v2, double v3,
double v4, const bool angular)
{
if (angular)
{
QList<double*> vals;
vals << &v1 << &v2 << &v3 << &v4;
NearestNeighbourAngle(vals);
}
double factor2 = factor * factor;
double factor3 = factor2 * factor;
bool logaritmic = false;
bool negative = false;
if ((v1 > 0 && v2 > 0 && v3 > 0 && v4 > 0) || (v1 < 0 && v2 < 0 && v3 < 0 && v4 < 0))
{
if (v1 < 0) negative = true;
double average = (v1 + v2 + v3 + v4) / 4.0;
if (average > 0)
{
double deviation = (fabs(v2 - v1) + fabs(v3 - v2) + fabs(v4 - v3)) / average;
if (deviation > 0.1)
{
v1 = log(fabs(v1));
v2 = log(fabs(v2));
v3 = log(fabs(v3));
v4 = log(fabs(v4));
logaritmic = true;
}
}
}
double value = 0.5
* ((2 * v2) + (-v1 + v3) * factor + (2 * v1 - 5 * v2 + 4 * v3 - v4) * factor2
+ (-v1 + 3 * v2 - 3 * v3 + v4) * factor3);
if (logaritmic)
{
if (negative) value = -exp(value);
else value = exp(value);
}
if (value > 1e20) value = 1e20;
if (value < -1e20) value = 1e20;
if (fabs(value) < 1e-20) value = 0.0;
if (angular)
{
return LimitAngle(value);
}
else
{
return value;
}
}
double cMorph::LinearInterpolate(const double factor, double v1, double v2, bool const angular)
{
if (angular)
{
QList<double*> vals;
vals << &v1 << &v2;
NearestNeighbourAngle(vals);
}
double value = v1 + ((v2 - v1) * factor);
if (angular)
{
return LimitAngle(value);
}
else
{
return value;
}
}
///////////////////
// Update the wheel position
void Car_UpdateWheel(carsim_t *psCar, CModel *pcTrack, int id)
{
wheel_t *w = &psCar->sWheels[id];
CVec relpos = w->cRelPos;
float p[3], t1[9], t2[9];
plane_t plane;
// Calculate the wheel 'top' pos
CVec wheelPos = psCar->X*relpos.GetX() + psCar->Y*relpos.GetY() + psCar->Z*relpos.GetZ() + psCar->cPos;
// Convert the wheel in car coords
relpos -= CVec(0,0,w->fRestLength);
w->cPos = psCar->X*relpos.GetX() + psCar->Y*relpos.GetY() + psCar->Z*relpos.GetZ() + psCar->cPos;
// This is for the rendering
w->fWheelZ = -w->fRestLength;
// Defaults
w->fSuspLength = w->fRestLength;
w->fPistonVelocity = 0;
// Check for collisions
p[0] = w->cPos.GetX(); p[1] = w->cPos.GetY(); p[2] = w->cPos.GetZ();
w->bCollision = false;
float pDist = 0;
if( pcTrack->getCDModel()->sphereCollision( p, w->fRadius ) ) {
float top=999999;
for(int n=0; n<pcTrack->getCDModel()->getNumCollisions(); n++) {
pcTrack->getCDModel()->getCollidingTriangles(n, t1,t2,false);
plane = CalculatePlane(CVec(t1[0],t1[1],t1[2]),
CVec(t1[3],t1[4],t1[5]),
CVec(t1[6],t1[7],t1[8]));
// If the normal is too great for the tyre, ignore the collision
if(DotProduct(plane.vNormal,psCar->Z) < 0.5f)
continue;
// Find the plane that is raised towards the wheel center the most
if( DotProduct(wheelPos, plane.vNormal) + plane.fDistance < top) {
// TODO: Check if the tyre is not too high (ie, over the suspension joint)
top = DotProduct(wheelPos, plane.vNormal) + plane.fDistance;
w->cNormal = plane.vNormal;
pDist = plane.fDistance;
}
w->bCollision = true;
}
}
w->fSpeed = 0;
// If not colliding, just leave
if(!w->bCollision)
return;
// Make the tyre sit on top of the road
float d = DotProduct(w->cPos, w->cNormal) + pDist;
w->fSuspLength = d;
relpos = w->cRelPos - CVec(0,0,w->fSuspLength);
w->fWheelZ = -w->fSuspLength;
w->cPos = psCar->X*relpos.GetX() + psCar->Y*relpos.GetY() + psCar->Z*relpos.GetZ() + psCar->cPos;
// Get the piston velocity for the suspension
CVec vel = CrossProduct(psCar->cPos-w->cPos, psCar->cAngVelocity) + psCar->cVelocity;
w->fPistonVelocity = (psCar->Z * DotProduct(psCar->Z,vel)).GetZ();
// Calculate the rotation speed of the tyre
CVec s = psCar->Y * DotProduct(psCar->cVelocity, psCar->Y);
w->fSpeed = VectorLength(s);
if(DotProduct(psCar->cVelocity, psCar->Y) > 0)
w->fSpin += w->fSpeed / w->fRadius;
else
w->fSpin -= w->fSpeed / w->fRadius;
// Rotation is based on the Torque from the engine
//w->fSpin += w->fTorque / w->fRadius;
// If the torque is greater then the slip torque for the surface, the wheel is slipping and the engine force
// is less
// Note: The slip torque is constant for now
if(id == 1) {
//tMainSR3.f1 = w->fTorque;
}
if(w->fTorque > 100) {
// Slipping
w->fEngineLoad = w->fTorque/50;
w->bSlip = true;
} else {
w->bSlip = false;
w->fEngineLoad = 0;
}
w->fSpin = LimitAngle(w->fSpin);
}
//-------------------------------------------
// とりあえずIK
void BoneModel::VMDIkAnimation()
{
//XMStoreFloat4()
//XMLoadFloat4()
if (mBone.empty())return;
if (mMotion.empty())return;
DWORD mBoneNum = mBone.size();
DWORD mIkNum = mIk.size();
// IK計算
for (DWORD i = 0; i < mIkNum; i++){
//{
// int i = 0;
Ik& ik = mIk[i];
UINT tg_idx = ik.target_bone_index;
UINT ik_idx = ik.bone_index;
for (UINT ite = 0; ite<ik.iterations; ++ite){
for (UINT chn = 0; chn<ik.chain_length; ++chn){
UINT link_idx = ik.child_bone_index[chn];//
if (link_idx >= mBoneNum)continue;
Bone& link_bone = mBone[link_idx];
//UINT link_pidx = link_bone.mIkBoneIdx;
UINT link_pidx = link_bone.mHierarchy.mIdxParent;
//if (link_bone.mIkBoneIdx != 0){
// continue;
//}
if (link_pidx >= mBoneNum)continue;
Bone& link_parent = mBone[link_pidx];
Bone& tg_bone = mBone[tg_idx];
(void)tg_bone;
Bone& ik_bone = mBone[ik_idx];
(void)ik_bone;
XMVECTOR target_wpos = mBone[tg_idx].mMtxPose.r[3];
XMVECTOR ik_wpos = mBone[ik_idx].mMtxPose.r[3];
XMVECTOR lp_wpos = link_parent.mMtxPose.r[3];
//Linkボーンのローカル空間に変換
XMVECTOR Determinant;
XMMATRIX inv_mtx = XMMatrixInverse(&Determinant, link_bone.mMtxPose);
XMVECTOR tg_pos = XMVector4Transform(target_wpos, inv_mtx);
XMVECTOR ik_pos = XMVector4Transform(ik_wpos, inv_mtx);
XMVECTOR lp_pos = XMVector4Transform(lp_wpos, inv_mtx);
// 回転軸と角度
XMVECTOR rot_axis = XMVectorSet(1, 0, 0, 0);
float ang = 0.0f;
bool same_dir = false;
if (!RotDir(tg_pos, ik_pos, ik.control_weight, &rot_axis, &ang)){
same_dir = true;
}
if (!same_dir){
//tg_dirをik_dirに一致させるための回転
XMVECTOR rot = XMQuaternionRotationAxis(rot_axis, ang);
XMVECTOR lrot = FloatToVector(link_bone.mRot);
XMVECTOR bone_rot_before = lrot;
link_bone.mRot = VectorToFloat(XMQuaternionMultiply(rot, lrot));
float dist_tg = XMVectorGetX(XMVector3Length(tg_pos));
float dist_ik = XMVectorGetX(XMVector3Length(ik_pos));
(void)dist_ik;
float dist_lp = XMVectorGetX(XMVector3Length(lp_pos));
(void)dist_lp;
float dist_pltg = XMVectorGetX(XMVector3Length(lp_pos - tg_pos));
float dist_plik = XMVectorGetX(XMVector3Length(lp_pos - ik_pos));
float dot_tgik = XMVectorGetX(XMVector3Dot(XMVector3Normalize(tg_pos), XMVector3Normalize(ik_pos)));
(void)dot_tgik;
// 回転制限
if (/*link.bLimit*/ 1){
XMVECTOR rotmax, rotmin;
//114.5916 = 2
float a = 2;// XM_PI / 180.0f * 57.25f;
rotmax = XMVectorSet(a, a, a, 0);//link.vMax;
rotmin = XMVectorSet(-a, -a, -a, 0);//link.vMin;
//名前に"ひざ"があったら回転制限
if (std::string::npos != link_bone.mStrName.find("ひざ")){
rotmax = XMVectorSet(-XM_PI / 180.0f*0.5f, 0, 0, 0);
rotmin = XMVectorSet(-XM_PI, 0, 0, 0);
}
struct IkLink{
XMFLOAT4 mMax;
XMFLOAT4 mMin;
};
IkLink link = { VectorToFloat(rotmax), VectorToFloat(rotmin) };
//Bone& link = link_bone;
link_bone.mRot = VectorToFloat(LimitAngle(FloatToVector(link_bone.mRot), rotmin, rotmax));
XMVECTOR angxyz = GetAngle(rot);
//膝を曲げるための仮処理 かなりてきとう
if (XMVectorGetX(angxyz) >= 0 &&
//0.9f < dot_tgik &&
//dist_tg > dist_ik &&
dist_pltg > dist_plik &&
link.mMax.x < 0 && link.mMax.y == link.mMin.y && link.mMax.z == link.mMin.z){
//親リンクの回転接平面（できるだけこの平面に近づけたほうがよりIK目標に近づける）
XMVECTOR lp_nor = XMVector3Normalize(-lp_pos);//平面の法線
//lp_norとの内積が0になる位置を目標にする
//２つあるので回転制限後の|内積|が小さいほう
XMVECTOR tng = XMVector3Cross(XMVectorSet(1, 0, 0, 0), lp_nor);
//+tngと-tngの２つ
XMVECTOR rot_axis0, rot_axis1;
float ang0 = 0, ang1 = 0;
// 回転軸をXに限定
rot_axis1 = rot_axis0 = XMVectorSet(1, 0, 0, 0);
XMVECTOR tdir = XMVector3Normalize(XMVectorSetX(tg_pos, 0));
tng = XMVector3Normalize(XMVectorSetX(tng, 0));
RotDir(tdir, tng, ik.control_weight, &rot_axis0, &ang0);
RotDir(tdir, -tng, ik.control_weight, &rot_axis1, &ang1);
if (XMVectorGetX(rot_axis0) < 0.0f)ang0 = -ang0;
if (XMVectorGetX(rot_axis1) < 0.0f)ang1 = -ang1;
//これは絶対違う ぴくぴく対策
float coef = (dist_pltg - dist_plik) / dist_tg;
if (coef > 1)coef = 1;
ang0 *= coef;
ang1 *= coef;
//ang0,1は現在の位置からの相対角度
// 回転制限を考慮した相対角度に
float angx_b = XMVectorGetX(GetAngle(bone_rot_before));
float angx_a0 = angx_b + ang0;
float angx_a1 = angx_b + ang1;
if (angx_a0 < link.mMin.x) angx_a0 = link.mMin.x;
if (angx_a0 > link.mMax.x) angx_a0 = link.mMax.x;
if (angx_a1 < link.mMin.x) angx_a1 = link.mMin.x;
if (angx_a1 > link.mMax.x) angx_a1 = link.mMax.x;
ang0 = angx_a0 - angx_b;
ang1 = angx_a1 - angx_b;
XMVECTOR rot0 = XMQuaternionRotationRollPitchYaw(ang0, 0, 0);
XMVECTOR rot1 = XMQuaternionRotationRollPitchYaw(ang1, 0, 0);
XMVECTOR tdir0 = XMVector3TransformCoord(tdir, XMMatrixRotationQuaternion(rot0));
XMVECTOR tdir1 = XMVector3TransformCoord(tdir, XMMatrixRotationQuaternion(rot1));
float d0 = XMVectorGetX(XMVectorAbs(XMVector3Dot(tdir0, lp_nor)));
float d1 = XMVectorGetX(XMVectorAbs(XMVector3Dot(tdir1, lp_nor)));
if (d0 < d1){
link_bone.mRot = VectorToFloat(XMQuaternionMultiply(rot0, bone_rot_before));
}
else{
link_bone.mRot = VectorToFloat(XMQuaternionMultiply(rot1, bone_rot_before));
}
}
}
}
//ワールド行列更新
link_bone.mMtxPose = SQTMatrix(FloatToVector(link_bone.mScale), FloatToVector(link_bone.mRot), FloatToVector(link_bone.mPos));
if (link_bone.mHierarchy.mIdxParent < mBoneNum){
link_bone.mMtxPose = XMMatrixMultiply(link_bone.mMtxPose, mBone[link_bone.mHierarchy.mIdxParent].mMtxPose);
}
// 子階層のリンク再計算
for (int lidown = chn - 1; lidown >= 0; --lidown){
UINT idx = ik.child_bone_index[lidown];
if (idx >= mBoneNum)continue;
Bone& linkb = mBone[idx];
linkb.mMtxPose = SQTMatrix(FloatToVector(linkb.mScale), FloatToVector(linkb.mRot), FloatToVector(linkb.mPos));
if (linkb.mHierarchy.mIdxParent < mBoneNum){
linkb.mMtxPose = XMMatrixMultiply(linkb.mMtxPose, mBone[linkb.mHierarchy.mIdxParent].mMtxPose);
}
}
mBone[tg_idx].mMtxPose = SQTMatrix(FloatToVector(mBone[tg_idx].mScale), FloatToVector(mBone[tg_idx].mRot), FloatToVector(mBone[tg_idx].mPos));
if (mBone[tg_idx].mHierarchy.mIdxParent < mBoneNum){
mBone[tg_idx].mMtxPose = XMMatrixMultiply(mBone[tg_idx].mMtxPose, mBone[mBone[tg_idx].mHierarchy.mIdxParent].mMtxPose);
}
}
}
//Bone& b = mBone[tg_idx];
//Bone& b2 = mBone[mBone[tg_idx].mHierarchy.mIdxParent];
//Bone& b3 = mBone[b2.mHierarchy.mIdxParent];
//int sa = 1;
//IKの計算結果を子階層に反映
//UpdatePose();
}
UpdatePose();
}
void A3DTransform::LimitAngles()
{
LimitAngle(xy);
LimitAngle(yz);
LimitAngle(xz);
}
void A3DPolarPoint::LimitAngles()
{
LimitAngle(angle1);
LimitAngle(angle2);
}
