0001: // ----------------------------------------------------------------------------
0002: //
0003: // rigidbody.cpp - 物体の運動制御
0004: // 
0005: // Copyright (c) 2001 if (imagire@gmail.com)
0006: // All Rights Reserved.
0007: //
0008: // ----------------------------------------------------------------------------
0009: #define STRICT
0010: 
0011: #include <windows.h>
0012: #include "main.h"
0013: #include "draw.h"
0014: #include "rigidbody.h"
0015: 
0016: CCar car;
0017: 
0018: // ----------------------------------------------------------------------------
0019: // 定数パラメータ
0020: const float max_speed = 1.0f;       // 最高速
0021: const float handle_max = PI/4.0f;   // 目いっぱいハンドルを切った時の角度
0022: 
0023: // ----------------------------------------------------------------------------
0024: // 剛体
0025: // ----------------------------------------------------------------------------
0026: void CRigidBody::Init()
0027: {
0028:     this->mass      = 1.0f;
0029:     D3DXMatrixIdentity(&this->IBody);
0030:     D3DXMatrixIdentity(&this->IBodyinv);
0031:     this->x         = D3DXVECTOR4(0.0f, 0.0f, 0.0f, 0.0f);
0032: 
0033:     this->R         = D3DXQUATERNION (0.0f, 0.0f, 0.0f, 1.0f);
0034:     this->p         = D3DXVECTOR4(0.0f, 0.0f, 0.0f, 0.0f);
0035:     this->L         = D3DXVECTOR3(0.0f, 0.0f, 0.0f);
0036: 
0037:     this->Calc2ndValues();
0038: }
0039: // ----------------------------------------------------------------------------
0040: // v や ω を計算
0041: void CRigidBody::Calc2ndValues()
0042: {
0043:     D3DXVec4Scale(&this->v, &this->p, 1.0f/this->mass);
0044:     
0045:     D3DXMATRIX m, mt;
0046:     D3DXMatrixRotationQuaternion(&m, &this->R);
0047:     D3DXMatrixTranspose(&mt, &m);
0048:     this->Iinv = m * this->IBodyinv * mt;
0049: 
0050:     D3DXVec3TransformNormal(&this->omega, &this->L, &this->Iinv);
0051: }
0052: // ----------------------------------------------------------------------------
0053: // 座標に時間変化を足しこむ
0054: void CRigidBody::Update(float dt)
0055: {
0056:     this->ComputeForceAndTorque(dt);
0057:     this->Calc2ndValues();
0058: 
0059:     this->x += dt * this->v;
0060:     this->p += dt * this->force;
0061:     this->L += dt * this->torque;
0062:     D3DXQUATERNION dr;
0063:     float arg = -dt*D3DXVec3Length(&this->omega);
0064:     if(arg < -0.001f || 0.001f < arg){
0065:         D3DXQuaternionRotationAxis( &dr, &this->omega, arg);
0066:         D3DXQuaternionMultiply(&this->R, &this->R, &dr);
0067:     }
0068: }
0069: // ----------------------------------------------------------------------------
0070: D3DXMATRIX CRigidBody::GetRotation() const
0071: {
0072:     D3DXMATRIX m;
0073: 
0074:     D3DXMatrixRotationQuaternion(&m, &this->R);
0075: 
0076:     return m;
0077: }
0078: // ----------------------------------------------------------------------------
0079: D3DXVECTOR4 CRigidBody::GetTranslate() const
0080: {
0081:     return this->x;
0082: }
0083: // ----------------------------------------------------------------------------
0084: void CRigidBody::SetTranslate(const D3DXVECTOR4 tr)
0085: {
0086:     this->x = tr;
0087: }
0088: // ----------------------------------------------------------------------------
0089: // 車
0090: // ----------------------------------------------------------------------------
0091: CCar::CCar()
0092: {
0093:     this->Init();
0094: }
0095: // ----------------------------------------------------------------------------
0096: void CCar::Init()
0097: {
0098:     CRigidBody::Init();
0099: 
0100:     // 車の形状を入力
0101:     float x = 15.0f;
0102:     float y =  5.0f;
0103:     float z = 25.0f;
0104:     this->mass = 1.0f;
0105:     this->IBody._11 = this->mass * (y*y+z*z) / 12.0f;
0106:     this->IBody._22 = this->mass * (x*x+z*z) / 12.0f;
0107:     this->IBody._33 = this->mass * (x*x+y*y) / 12.0f;
0108:     this->IBodyinv._11 = 1.0f / this->IBody._11;
0109:     this->IBodyinv._22 = 1.0f / this->IBody._22;
0110:     this->IBodyinv._33 = 1.0f / this->IBody._33;
0111: 
0112:     this->dt = 0;
0113:     this->time = timeGetTime();
0114:     this->speed = 0.0f;
0115:     this->handle = 0;
0116: }
0117: // ----------------------------------------------------------------------------
0118: void CCar::Control()
0119: {
0120:     this->dt = -this->time;
0121:     this->time = timeGetTime();
0122:     this->dt += this->time;
0123: 
0124:     if(0==this->dt) return;// 速すぎ？
0125: 
0126:     // 最高速度、最低速度をつける
0127:     if(+max_speed < this->speed) this->speed = +max_speed;
0128:     if(this->speed < -max_speed) this->speed = -max_speed;
0129: 
0130:     // ハンドルの向きを調整する
0131:     if(this->dt){
0132:         if(+handle_max < this->handle*this->dt) this->handle = +handle_max/this->dt;
0133:         if(this->handle*this->dt < -handle_max) this->handle = -handle_max/this->dt;
0134:     }
0135:     
0136:     this->Update((float)this->dt);
0137: 
0138:     this->handle *= (float)exp(-(float)this->dt/1000.0f);// 入力が無かったら、ハンドルは戻す
0139: }
0140: // ----------------------------------------------------------------------------
0141: void CCar::AddSpeed(float acc)
0142: {
0143:     this->speed += acc;
0144: }
0145: // ----------------------------------------------------------------------------
0146: void CCar::AddHandle(float dh)
0147: {
0148:     this->handle += dh;
0149: }
0150: // ----------------------------------------------------------------------------
0151: // 外力と中心へかかる向きから、重心へ力とトルクを求める
0152: static void AddForceAndTorque(D3DXVECTOR4 *force, D3DXVECTOR3 *torque, const D3DXVECTOR4 power, const D3DXVECTOR4   pos)
0153: {
0154:     D3DXVECTOR4 p_pall = (D3DXVec4Dot(&pos, &power)/D3DXVec4Dot(&pos, &pos)) * pos;
0155:     D3DXVECTOR4 p_vert = power - p_pall;
0156:     D3DXVECTOR3 out;
0157: 
0158:     *force  += p_pall;
0159:     
0160:     D3DXVec3Cross(&out,(D3DXVECTOR3*)&pos, (D3DXVECTOR3*)&p_vert); 
0161:     *torque += out / D3DXVec4Dot(&pos, &pos);
0162: }
0163: // ----------------------------------------------------------------------------
0164: void CCar::ComputeForceAndTorque(float dt)
0165: {
0166:     if(dt < 0.0000000000001f) return;
0167: 
0168:     int i;
0169:     // 車の中心位置（重心とモデルの中心がずれている補正）
0170:     const float cy = 2.2f;
0171:     D3DXVECTOR4 center = D3DXVECTOR4(0.0f, cy, 0.0f, 0.0f);
0172:     
0173:     // タイヤの位置
0174:     D3DXVECTOR4 tire[4] = {
0175:         D3DXVECTOR4( 4.4f, 0.0f, 6.8f, 0.0f),// 前右
0176:         D3DXVECTOR4(-4.4f, 0.0f, 6.8f, 0.0f),// 前左
0177:         D3DXVECTOR4( 4.4f, 0.0f,-6.8f, 0.0f),// 後右
0178:         D3DXVECTOR4(-4.4f, 0.0f,-6.8f, 0.0f),// 後左
0179:     };
0180:     D3DXMATRIX mR;
0181:     D3DXMatrixRotationQuaternion(&mR, &this->R);
0182:     
0183:     for(i = 0; i < 4; i++){
0184:         tire[i] -= center;
0185:         D3DXVec4Transform(&tire[i], &tire[i], &mR);
0186:         tire[i] += center;
0187:     }
0188:     
0189:     // 車の進行方向
0190:     D3DXVECTOR4 dir    = D3DXVECTOR4(0.0f, 0.0f, 1.0f, 0.0f);
0191:     D3DXVec4Transform(&dir, &dir, &mR);
0192:     
0193:     // 車の進行方向の速度成分
0194:     float spd = D3DXVec4Dot(&dir, &this->v);
0195:     D3DXVECTOR4 v_pall = spd * dir;
0196:     if(spd < 0.0f)spd = -spd;   // spd は速さ
0197: 
0198:     // 力のリセット
0199:     this->force     = D3DXVECTOR4(0.0f, 0.0f, 0.0f, 0.0f);
0200:     this->torque    = D3DXVECTOR3(0.0f, 0.0f, 0.0f);
0201: 
0202:     // タイヤが触れたか判定
0203:     bool hit[4];
0204:     float  hit_cnt = 0.0f;  // 触れた回数
0205:     for(i = 0; i < 4; i++){
0206:         D3DXVECTOR4 p = tire[i] + this->x;
0207:         if(-FLOOR_SIZE <= p.x && p.x <= +FLOOR_SIZE
0208:         && -FLOOR_SIZE <= p.z && p.z <= +FLOOR_SIZE
0209:         && p.y < 0.0f){
0210:             hit_cnt += 1.0f;
0211:             hit[i] = true;
0212:         }else{
0213:             hit[i] = false;
0214:         }
0215:     }
0216: 
0217:     // 前輪
0218:     for(i = 0; i < 2; i++){
0219:         if(hit[i]){
0220:             D3DXVECTOR4 power = D3DXVECTOR4(0.0f, 0.0f, 0.0f, 0.0f);
0221:             float dy = -(tire[i].y + this->x.y) / hit_cnt;
0222:             power[1] += dy;                                  // 床に沈んだ抗力
0223:             // 陀角の調整
0224:             D3DXVECTOR4 d_handle    = D3DXVECTOR4(0.0f, 0.0f, 1.0f, 0.0f);
0225:             D3DXMATRIX  m;
0226:             D3DXMatrixRotationY(&m, dt*this->handle);
0227:             D3DXVec4Transform(&d_handle, &d_handle, &m);
0228:             D3DXVec4Transform(&d_handle, &d_handle, &mR);
0229:             D3DXVECTOR4 v_handle = D3DXVec4Dot(&d_handle, &this->v) * d_handle;
0230:             v_handle = this->v - v_handle;  //v_handle は、速度と垂直成分
0231:             power -= 0.5f*dt*v_handle;
0232: 
0233:             AddForceAndTorque(&this->force, &this->torque, power, center-tire[i]);
0234:         }
0235:     }
0236:     // 後輪
0237:     for(i = 2; i < 4; i++){
0238:         if(hit[i]){
0239:             D3DXVECTOR4 power = D3DXVECTOR4(0.0f, 0.0f, 0.0f, 0.0f);
0240:             float dy = -(tire[i].y + this->x.y) / hit_cnt;
0241:             power[1] += dy;                                  // 床に沈んだ抗力
0242:             power[0] += 0.01f*(this->speed-spd)*dir[0];     // エンジンによる駆動
0243:             power[2] += 0.01f*(this->speed-spd)*dir[2];     // エンジンによる駆動
0244: 
0245:             AddForceAndTorque(&this->force, &this->torque, power, center-tire[i]);
0246:         }
0247:     }
0248: 
0249:     // フレームに依存しない動きの為の補正
0250:     force  /= (dt*dt);
0251:     torque /= (dt*dt);
0252: 
0253:     // 重力
0254:     force[1] -= 0.00002f;
0255:     
0256:     // 空気抵抗
0257:     force += -0.01f*this->v / dt;
0258:     
0259:     if(0.000001f*dt < this->p[1]) this->p[1] = 0.000001f*dt;    // はねないように強制的に調整
0260: }
0261: // ----------------------------------------------------------------------------
0262: // 外部とのインターフェイス
0263: // ----------------------------------------------------------------------------
0264: void MyMsgProc(UINT wParam)
0265: {
0266:     switch(wParam){
0267:     case VK_UP:
0268:         car.AddSpeed(+0.01f);
0269:         break;
0270:     case VK_DOWN:
0271:         car.AddSpeed(-0.01f);
0272:         break;
0273:     case VK_RIGHT:
0274:         car.AddHandle(+PI/12.0f);
0275:         break;
0276:     case VK_LEFT:
0277:         car.AddHandle(-PI/12.0f);
0278:         break;
0279:     case VK_RETURN:
0280:     case VK_SPACE:
0281:         // 原点に戻す
0282:         car.Init();
0283:         break;
0284:     }
0285: }
0286: