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になる位置を目標にする //2つあるので回転制限後の|内積|が小さいほう XMVECTOR tng = XMVector3Cross(XMVectorSet(1, 0, 0, 0), lp_nor); //+tngと-tngの2つ 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); }