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