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