游戏引擎设计 - 物理（Crapell Game Engine Design

这是一个小型的游戏物理引擎，大部分功能都有比如刚体、连接、布娃娃系统。

稍微有些特色的地方：

添加了浮力的效果，

布娃娃系统可以响应预制动画和物理双重模拟，

添加了一个自适应龙形动画。

从3dsmax中导出物理场景比较方便。

浮力效果

龙形动画，这是一条可以骑乘的龙，配合按键动作龙会做出相应的拐弯、爬升、翻滚等动作，动作由数学曲线计算产生带有随机舞动的特征，龙身附带刚体。

不足的地方：

没有做连续碰撞检测，快速运动的物体可能发生穿透，快速运动的连接关节不够稳定。

赛车部分，四个车轮通过关节同车体连接，高速运动时不够稳定，关于这一点bullet引擎中也有提到，bullet引擎有一种raycast的方法来模拟车辆。

虽然布娃娃的物理模拟和预制动画之间的切换加了过渡，但切换之后只能是单独控制，联合控制表现不好。

//=============================

理论基础： 参考《工程力学》相关书籍

力的平行四边形法则

作用在物体上同一点的两个力可以合成为一个合力。两个分力为边构造平行四边形，对角线为合力。

力的可传性

作用在刚体上的力可以沿着力的作用线任意移动作用点而不改变力的效果。

力矩

在平面力系中，力矩为力的大小和力臂的乘积（支点到作用点的位移和力的叉乘）。空间力系中‘力矩矢’为力的矩心(支点)到作用点的矢径和力的叉乘。空间力系中力对轴的矩，力对点的矩，力对通过点的轴的矩都可以通过投影计算。

力偶

大小相等方向相反且不共线的两个平衡力组成一个力偶。力偶只能使物体产生转动，而不能使物体发生移动。在平面力系中，力偶为力的大小和力偶臂的乘积（叉乘的大小）。空间力系中‘力偶矩矢’为力的力偶臂和力的叉乘。

力的平移定理

可以把作用在刚体上点A的力F平移到任意点B，但必须同时附加一个力偶，附加力偶的矩等于力F对于点B的矩。本定理是力系简化的理论依据，因此任意n个不同作用点的外力可以平移到同一作用点(通常为重心)变成汇交力系统。新的力系只有一个主矢和主矩。

重心

刚体平行力系的中心，平行力系绕中心旋转一定角度，合力也会选择相同角度。

牛顿运动定律

在直线运动中 F=ma，在旋转运动，则有就τ=Ια。

动量

动量（Momentum）又称线性动量（Linear Momentum）表示为物体的质量和速度的乘积P=m*v，指的是这个物体在它运动方向上保持运动的趋势，单位kg·m/s。质点组的动量为组内各质点动量的矢量和。动量是一个守恒量，力反映了动量传递快慢的情况。

冲量(impulse)是力与其作用时间的乘积I=F*t，表述了力对时间的积累效应。动量是状态量不同，冲量是一个过程量。物体所受合外力的冲量等于它的动量的增量即动量定理P'-P=I。

动能

物体由于运动而具有的能量 mv^/2，将速度v和质量m，用转动力学的定义取代：v=wr，m=I/r^。K=Iw^/2。

惯性张量

对于三维空间中任意一参考点 Q 与以此参考点为原点的直角坐标系 Qxyz ，一个刚体的惯性张量

约束力

面约束：约束力方向为面的法线方向。

链条约束：约束力方向沿着链条背离物体。

铰链、轴承约束：约束力方向垂直于轴线并通过轴心。

球铰链约束：约束力通过接触点和球心。

优化策略

1，利用包围盒子快速剔除碰撞对。再利用包围盒子快速剔除碰撞对中一个刚体的面，再快速剔除另一个刚体的点。这样复杂度降低三个数量级。更加紧密的包围盒子会使效率显著提高。

2，对具有大体积和大量面的静态体(类似模型地面)使用bsptree。因为大体积意味着可能和许多刚体发生碰撞对，大量面意味着每个碰撞对检测的面和点都很多（用bsptree比用包围盒来剔除更高效），同时静态体意味着可以避免bsptree动态更新的开销。其它动态小体积刚体实际上只和其它两三个刚体发生碰撞对，因此碰撞对可以用包围盒子快速剔除。即使小体积刚体有很多面也可以通过包围合快速剔除大部分的面和点，因此不需要bsptree来优化，同时减少了bsptree动态更新的开销。

3，缓存计算结果

4，除了地形外，其它刚体一律使用200面以下的简模计算碰撞，或使用球体碰撞。

稳定性：

1，过度反弹：存在多个接触点时，每个接触点计算分力。

2，过度穿透：点和面碰撞的同时，计算边和面的碰撞。

3，凹体碰撞：。

c++实现：

提醒一个曾经踩到的坑，direct3d重置设备后（比如切换后台）可能导致double的双精度计算变成了float的单精度，如果定时器没处理好，程序会产生莫名的震荡。这个坑直接导致了一堆可能不需要的代码。

1，定义刚体

//========================================================
//  @Date:     2016.05
//  @File:     Include/Physic/RigidBody.h
//  @Brief:     RigidBody
//  @Author:     LouLei
//  @Email:  twopointfive@163.com
//  @Copyright (Crapell) - All Rights Reserved
//========================================================#pragma once#ifndef __RigidBody_H__
#define __RigidBody_H__#include "Math/MathLib.h"#define PROFILEFUN_Physic
//#define PROFILEFUN_Physic PROFILEFUN  大量的log帧速率不掉 但可能每0.5s就卡一下//#define TestNotValidNum_Physic
#define TestNotValidNum_Physic  TestNotValidNumnamespace Physic
{#define StaticMass 0class PhyObject;
class Collide;
class Joint;
class PhyMesh;//max 中尽量确保质心在原点，否则形如不倒翁
//
class RigidBody
{friend class Collide;friend class Joint;friend class JointBall;friend class JointHinge;friend class JointUniversal;friend class PhyObject;friend class PhysicWorld;public:enum {CollideMesh   = 1 << 0,   //碰撞类型CollideSphere = 1 << 1,BodyBox      = 1 << 2,    //转动惯量类型BodySphere   = 1 << 3,BodyCylinder = 1 << 4,NUM_JOINTS = 6,};RigidBody(PhyMesh *mesh,float mass,float restitution,float friction,int flag);
//protected:~RigidBody();
public:void SetPos(const vec3 &pos);//void SetRot(const vec3 &rot);//设置速度，静态体不设置速度不会和frozen的动态体发生碰撞void SetVelocity(const vec3 &lineVelocity,const vec3 &angVelocity);//矩阵不能带缩放，否则缩放越大,旋转误差越大void SetMat(const mat4 &mat); //起到SetPos+SetRot的作用//瞬间增加冲量, 动量定理:动量(mv)的改变=冲量(F*t)void AddImpulse(const vec3 &point,const vec3 &impulse);  void ClearExtForce();void AddExtForce(const vec3 &point,const vec3 &force); //protected:friend int rigidbody_cmp_x(const void *a,const void *b);void CalcForce(float ifps);void FindContacts(float ifps);void IntegrateVelocity(float ifps);void IntegratePos(float ifps);int  ContactsResponse(float ifps,int zero_restitution = 0);int  GetJointNum();bool HasJoint(RigidBody* other);void RemoveJoint(Joint *joint);void EditRender();const vec3& getWMin(int s = -1);const vec3& getWMax(int s = -1);const vec3& GetWCenter(int s = -1);float getRadius(int s = -1);float getVolume(int s = -1);PhyObject* GetOwnerObj();void       SetOwnerObj(PhyObject* owner);int      m_type;Collide* m_collide;float m_mass;           //质量float m_ringFac;       //弹性系数float m_fricFac;       //摩擦系数vec3  m_massCenter;     //重心vec3  m_forces;         //合外力(主矢) 经过重心 只影响线速度vec3  m_torques;        //合力矩,角加速度(主矩)vec3  m_lineVelocity;   //线速度vec3  m_angVelocity;    //角速度 长度==大小 方向==旋转轴的方向； 角加速度==主矩vec3  m_angMomentum;    //角动量vec3  m_extForces;         //合外力vec3  m_extTorques;        //合力矩mat3  iBodyInertiaTensor; //局部转动惯量逆mat3  iWorldInertiaTensor;//世界转动惯量逆  mat3  m_rotMat;         //旋转mat4  transform;mat4  itransform;int   m_frozen;float m_frozenTime;int   m_frozenCollideObjNum;int   m_immovable;int   m_isIdentity;    //等于physicMesh的radiusfloat m_radius;//初始重置vec3  m_resetPos;vec3  m_resetRot;PhyMesh*   m_phyMesh;#define m_phyWorld m_phyWorldPhysicWorld* m_phyWorld;protected:int        m_jointNum;Joint*     m_joints[NUM_JOINTS];RigidBody* m_joinedRigidbodies[NUM_JOINTS];PhyObject* m_owner;int  ID;
};}
#endif

#include "General/Pch.h"
#include "Physic/FindContacts.h"
#include "Physic/PhysicWorld.h"
#include "Physic/Joint.h"
#include "Physic/PhyObject.h"
#include "Physic/FindContacts.h"
#include "Physic/RigidBody.h"
#include "Physic/PhyMesh.h"
#include "General/Pce.h"
#include "Physic/PhyObjWater.h"namespace Physic
{RigidBody::RigidBody(PhyMesh *mesh, float mass, float restitution, float friction, int flag)
:m_mass(mass)
,m_collide(NULL)
,m_ringFac(restitution)
,m_fricFac(friction)
,m_frozen(0)
,m_frozenTime(0.0)
,m_frozenCollideObjNum(0)
,m_jointNum(0)
,m_immovable(0)
,m_isIdentity(1)
,m_phyWorld(NULL)
,ID(-1)
,m_owner(NULL)
{m_phyMesh = mesh;m_type = flag;m_collide = new Collide(m_phyMesh);if (m_phyMesh){m_radius = m_phyMesh->getRadius();if (mass <=0){mass = 999;}//转动惯量的缩放是二次方的if(flag && BodyBox)  //body 仅仅是计算转动惯量使用？ 没有自动计算不规则模型转动惯量的方法？{vec3 min = m_phyMesh->getMin();vec3 max = m_phyMesh->getMax();vec3 v = (max - min) / 2.0;mat3 inertiaTensor;inertiaTensor[0] = 1.0f / 12.0f * mass * (v.y * v.y + v.z * v.z);inertiaTensor[4] = 1.0f / 12.0f * mass * (v.x * v.x + v.z * v.z);inertiaTensor[8] = 1.0f / 12.0f * mass * (v.x * v.x + v.y * v.y);iBodyInertiaTensor = inertiaTensor;iBodyInertiaTensor.Inverse();}else if(flag && BodySphere){mat3 inertiaTensor;inertiaTensor[0] = 2.0f / 5.0f * mass * m_radius * m_radius;inertiaTensor[4] = 2.0f / 5.0f * mass * m_radius * m_radius;inertiaTensor[8] = 2.0f / 5.0f * mass * m_radius * m_radius;iBodyInertiaTensor = inertiaTensor;iBodyInertiaTensor.Inverse();}else if(flag && BodyCylinder){vec3 min = mesh->getMin();vec3 max = mesh->getMax();float radius = max.x - min.x;float height = max.z - min.z;mat3 inertiaTensor;inertiaTensor[0] = 1.0f / 12.0f * mass * height * height;inertiaTensor[4] = 1.0f / 12.0f * mass * height * height;inertiaTensor[8] = 1.0f / 2.0f  * mass * radius * radius;iBodyInertiaTensor = inertiaTensor;iBodyInertiaTensor.Inverse();}//将多个surface 合并到一个大 surface//不合并的一个好处 可以作为天然的bsp分割？ 除了静态体 动态更新的bsp树消耗太大//m_vertexNum = 0;//m_trigonNum = 0;//m_edgeNum = 0;//for(int i=0;i<m_rigidBodyNum;i++)//{// m_vertexNum += m_rigidBodies[i]->m_vertexNum;//    m_trigonNum += m_rigidBodies[i]->m_trigonNum;//    m_edgeNum += m_rigidBodies[i]->m_edgeNum;//}//m_vertices=new vec3[m_vertexNum];m_trigons = new Triangles[m_trigonNum];//m_trigons = new PhysicTriangle[m_trigonNum];//m_edges = new PhysicEdge[m_edgeNum];//m_ringFac=m_rigidBodies[0]->m_ringFac;//m_fricFac=m_rigidBodies[0]->m_fricFac;//InitVerticesPos();计算合并后的质量和重心位置//int index = 0;//int indexEdge = 0;//for(int i=0;i<m_rigidBodyNum;i++)//{//    for(int j=0;j<m_rigidBodies[i]->m_edgeNum;j++)//   {//     m_edges[indexEdge++]=(m_rigidBodies[i]->m_edges[j]);//        //m_edges[indexEdge++]=(Edges(&m_vertices[0],&m_vertices[1]));// }// for(int j=0;j<(int)m_rigidBodies[i]->m_trigonNum;j++)//    {//     m_trigons[index++]=(m_rigidBodies[i]->m_trigons[j]);//    }// m_mass+=m_rigidBodies[i]->m_mass;//    m_massCenter+=m_rigidBodies[i]->m_mass*m_rigidBodies[i]->m_massCenter;//}//m_massCenter=m_massCenter/m_mass;初始化刚体碰撞平面for(int i=0;i<m_trigonNum;i++){m_planes[i].Set(m_trigons[i].point1,m_trigons[i].point2,m_trigons[i].point3,true);}计算合并后的惯量//float Ixx=0,Iyy=0,Izz=0;//vec3 temp;//for(int i=0;i<m_rigidBodyNum;i++)//{//   temp=m_rigidBodies[i]->m_massCenter-m_massCenter;// //Ixx+=RBArray[i]->inertia._11+RBArray[i]->mass*(temp.y*temp.y+temp.z*temp.z);//  //Iyy+=RBArray[i]->inertia._22+RBArray[i]->mass*(temp.x*temp.x+temp.z*temp.z);//  //Izz+=RBArray[i]->inertia._33+RBArray[i]->mass*(temp.x*temp.x+temp.y*temp.y);//  Ixx+=m_rigidBodies[i]->m_inertia[0]+m_rigidBodies[i]->m_mass*(temp.y*temp.y+temp.z*temp.z);// Iyy+=m_rigidBodies[i]->m_inertia[5]+m_rigidBodies[i]->m_mass*(temp.x*temp.x+temp.z*temp.z);// Izz+=m_rigidBodies[i]->m_inertia[10]+m_rigidBodies[i]->m_mass*(temp.x*temp.x+temp.y*temp.y);//}//float m[] ={Ixx,0,0,0,//    0,Iyy,0,0,//    0,0,Izz,0,//    0,0,0,1};//m_inertia=m;D3DXMatrixInverse(&InvInertia,NULL,&inertia);//m_invInertia = m_inertia;//m_invInertia.Inverse();}}RigidBody::~RigidBody()
{//if (m_owner)//{//    m_owner->SetRigidBody(NULL);  OnRigidRemove//    m_owner = NULL;//}if (ID>=0){m_phyWorld->RemoveRigidBody(this);}//m_phyMesh时静态模型可以共用//共用的话需要外部负责释放//此处释放的话 需要注意不能共用m_phyMeshSafeDelete(m_phyMesh);SafeDelete(m_collide);
}void RigidBody::SetPos(const vec3 &pos)
{//update bspm_isIdentity = 0;m_massCenter = pos;transform[12] = m_massCenter.x;transform[13] = m_massCenter.y;transform[14] = m_massCenter.z;itransform = transform;itransform.Inverse();m_collide->UpdateSurface(this);m_frozen = 0;m_frozenTime = 0.0f;m_immovable = 1;TestNotValidNum_Physic(m_massCenter);
}//设置速度，静态体不设置速度不会和frozen的动态体发生碰撞
void RigidBody::SetVelocity(const vec3 &lineVelocity,const vec3 &angVelocity)
{m_lineVelocity = lineVelocity;m_angVelocity = angVelocity;m_angMomentum = vec3(0, 0, 0);m_frozen = 0;m_frozenTime = 0.0f;m_immovable = 1;TestNotValidNum_Physic(m_lineVelocity);TestNotValidNum_Physic(m_angVelocity);
}void RigidBody::SetMat(const mat4 &m)
{//update bspm_isIdentity = 0;//vec3 old_pos = m_massCenter;//m_massCenter = m * vec3(0, 0, 0);  //质心在原点m_massCenter = m.GetTranslate();//m_rotMat = mat3(m);m_rotMat = m.RotationPart();//mat4 old_transform = transform;transform  = m;itransform = transform;itransform.Inverse();//velocity = (pos - old_pos) * Engine::ifps * 100.0f;m_lineVelocity = vec3(0, 0, 0);m_angVelocity  = vec3(0, 0, 0);m_angMomentum  = vec3(0, 0, 0);iWorldInertiaTensor.Identity();m_frozen     = 0;m_frozenTime = 0.0f;m_immovable  = 1;m_collide->UpdateSurface(this);TestNotValidNum_Physic(m);
}void RigidBody::AddImpulse(const vec3 &point, const vec3 &impulse)
{if (m_mass==StaticMass)return;m_lineVelocity += impulse / m_mass;m_angMomentum  += Cross(point - m_massCenter, impulse);//作用力对重心的力矩m_angVelocity   = iWorldInertiaTensor * m_angMomentum;TestNotValidNum_Physic(point);TestNotValidNum_Physic(impulse);
}
void RigidBody::ClearExtForce()
{m_extForces  = vec3(0, 0, 0);m_extTorques = vec3(0, 0, 0);
}void RigidBody::AddExtForce(const vec3 &point,const vec3 &force)
{m_extForces  += force;vec3 rc = point - m_massCenter;//力矩m_extTorques += Cross(rc , force);
}
void RigidBody::CalcForce(float ifps)
{//m_forces  = vec3(0, 0, 0);//m_torques = vec3(0, 0, 0);m_forces  = m_extForces;m_torques = m_extTorques;if(!m_frozen) {m_forces += PhysicWorld::Gravity * m_mass;//重力}//浮力if (m_mass!=StaticMass){PhysicObjectWater* objWater;PhyObject** objIt;objIt = m_phyWorld->m_phyObjects2;for(int b_ = 0; b_ < m_phyWorld->m_phyObjectNum2; b_++,objIt++){objWater = dynamic_cast<PhysicObjectWater*>(*objIt);if (objWater){objWater->CalcBuoyancy(*this);}}}m_forces -= m_lineVelocity * 0.1f;  //风阻？
}void RigidBody::FindContacts(float ifps)
{PROFILEFUN_Physic("RigidBody::FindContacts();",0.0f,ALWAYSHIDE);static vec3 old_velicity;static vec3 old_angularMomentum;static vec3 old_angularVelocity;static vec3 old_pos;static mat3 old_orientation;static mat3 old_iWorldInertiaTensor;if (m_mass!=StaticMass){CopyVec3(old_velicity        , m_lineVelocity);CopyVec3(old_angularMomentum , m_angMomentum);CopyVec3(old_angularVelocity , m_angVelocity);CopyVec3(old_pos             , m_massCenter);old_orientation         = m_rotMat;old_iWorldInertiaTensor = iWorldInertiaTensor;//预测新位置、速度IntegrateVelocity(ifps);IntegratePos(ifps);}//静态的不能被撞却可以撞别人m_collide->FindContacts(this);if (m_mass!=StaticMass){CopyVec3(m_lineVelocity , old_velicity); //恢复旧值CopyVec3(m_angMomentum  , old_angularMomentum);CopyVec3(m_angVelocity  , old_angularVelocity);CopyVec3(m_massCenter   , old_pos);m_rotMat            = old_orientation;iWorldInertiaTensor = old_iWorldInertiaTensor;}
}void RigidBody::IntegrateVelocity(float ifps)
{if (m_mass==StaticMass){//速度由外部关键帧信息设置return;}m_lineVelocity += m_forces * ifps / m_mass;float vel = m_lineVelocity.Length();if(vel > PhysicWorld::VelocityMax) {m_lineVelocity *= PhysicWorld::VelocityMax / vel;}m_angMomentum += m_torques * ifps;m_angVelocity = iWorldInertiaTensor * m_angMomentum; //TestNotValidNum_Physic(m_lineVelocity);TestNotValidNum_Physic(m_angVelocity);
}void RigidBody::IntegratePos(float ifps)
{//PROFILEFUN_Physic("RigidBody::IntegratePos();",0.0f,ALWAYSHIDE);//update bspif (m_mass==StaticMass){//transform由外部关键帧信息设置m_massCenter.x = transform[12];m_massCenter.y = transform[13];m_massCenter.z = transform[14];m_rotMat = transform;mat3 temp = m_rotMat;temp.Transpose();iWorldInertiaTensor = m_rotMat * iBodyInertiaTensor * temp;//return;//if (m_isIdentity = 0)//{// itransform = transform;//  itransform.Inverse();// m_collide->UpdateSurface(this);//}}else{m_massCenter += m_lineVelocity * ifps;//quat omegaQ(m_angVelocity.x, m_angVelocity.y, m_angVelocity.z, 0);//m_rotQ += (omegaQ*(0.5f*ifps)).MultR(m_rotQ);//m_rotQ.Normalize();//vec3  rightDir = m_angVelocity;//rightDir.Normalize();//float rotAngleRad = m_angVelocity.Length();//quat omegaQ;//omegaQ.FromAxisAngle(rightDir, rotAngleRad);//m_rotQ = omegaQ.MultR(m_rotQ);  //m_rotQ.Normalize();//{//   vec3  rightDir = m_angVelocity;//  rightDir.Normalize();// float rotAngleRad = m_angVelocity.Length();//  mat3 rot;// rot.FromAxisAngle(rightDir,rotAngleRad*ifps);// m_rotMat = rot * m_rotMat;//}{mat3 rot; //新增旋转rot[0] = 0.0;rot[3] = -m_angVelocity[2];rot[6] =  m_angVelocity[1];rot[1] =  m_angVelocity[2];rot[4] = 0.0;rot[7] = -m_angVelocity[0];rot[2] = -m_angVelocity[1];rot[5] =  m_angVelocity[0];rot[8] = 0.0;m_rotMat += (rot * m_rotMat) * ifps;m_rotMat.Orthonormalize();}mat3 temp = m_rotMat;temp.Transpose();iWorldInertiaTensor = m_rotMat * iBodyInertiaTensor * temp;transform = mat4(m_rotMat);transform[12] = m_massCenter.x;transform[13] = m_massCenter.y;transform[14] = m_massCenter.z;}itransform = transform;itransform.Inverse();m_collide->UpdateSurface(this);//TestNotValidNum_Physic(m_massCenter);//TestNotValidNum_Physic(m_forces);//TestNotValidNum_Physic(m_torques);//TestNotValidNum_Physic(m_lineVelocity);//TestNotValidNum_Physic(m_angVelocity);//TestNotValidNum_Physic(m_angMomentum);    //TestNotValidNum_Physic(iBodyInertiaTensor);//TestNotValidNum_Physic(iWorldInertiaTensor);//TestNotValidNum_Physic(m_rotMat);TestNotValidNum_Physic(transform);//TestNotValidNum_Physic(itransform);//TestNotValidNum_Physic(m_radius);
}int  RigidBody::ContactsResponse(float ifps, int zero_restitution)
{//PROFILEFUN_Physic("RigidBody::ContactsResponse();",0.0f,ALWAYSHIDE);//刚体直接碰撞冲量:速度越大反弹越大，穿刺距离不一定就大 //刚体之间的摩擦冲量:速度越大压力越大，摩擦越大，//添加冲量而不是力，因为碰撞时间太短，力无法计算，只能累积到冲量里面。//if (m_mass==StaticMass) //{//    return 1;//还要处理对方//}int done = 1;//for each contactfor(int i = 0; i < m_collide->m_contactNum; i++){Collide::Contact *c = &m_collide->m_contacts[i];RigidBody *rb = c->rigidBody;if(rb==NULL) continue;if(m_mass==StaticMass && rb->m_mass==StaticMass)continue;//if(1)//rb->m_mass!=StaticMass)     // rigidbody - rigidbody contact{vec3 r0  = c->point - m_massCenter;vec3 r1  = c->point - rb->m_massCenter;//旋转速度+线速度=质点实际速度vec3 vel = (Cross(m_angVelocity, r0) + m_lineVelocity) - (Cross(rb->m_angVelocity, r1) + rb->m_lineVelocity);float normal_vel = c->normal.Dot(vel); //质点法线速度分量if(normal_vel > -_EPSILON)continue;float numerator;if(!zero_restitution) {//使用弹性系数numerator = -(1.0f + m_ringFac) * normal_vel;}else {//使用穿透速度numerator = -normal_vel + c->depth * PhysicWorld::PenetrationSpeed / ifps;}if(numerator < _EPSILON)continue;//切向分量vec3 tangent = -(vel - c->normal * normal_vel);done = 0;float denominator; //冲量分母if(m_mass==StaticMass){denominator = 1.0f / rb->m_mass + c->normal.Dot(Cross(rb->iWorldInertiaTensor * Cross(r1, c->normal), r1));}else if(rb->m_mass==StaticMass){denominator = 1.0f / m_mass + c->normal.Dot(Cross(iWorldInertiaTensor * Cross(r0, c->normal), r0));}else{denominator = 1.0f / m_mass + 1.0f / rb->m_mass+ c->normal.Dot(Cross(iWorldInertiaTensor * Cross(r0, c->normal), r0))+ c->normal.Dot(Cross(rb->iWorldInertiaTensor * Cross(r1, c->normal), r1));}vec3 impulse = c->normal * numerator / denominator;if(m_frozen == 0 && m_immovable == 0 && m_mass!=StaticMass){//AddImpulse(c->point,impulse);m_lineVelocity += impulse / m_mass;m_angMomentum  += Cross(r0, impulse);m_angVelocity   = iWorldInertiaTensor * m_angMomentum;TestNotValidNum_Physic(m_angVelocity);TestNotValidNum_Physic(m_angVelocity);}if(rb->m_frozen == 0 && rb->m_immovable == 0 && rb->m_mass!=StaticMass){//rb->AddImpulse(c->point,-impulse);rb->m_lineVelocity -= impulse / rb->m_mass;rb->m_angMomentum  -= Cross(r1, impulse);rb->m_angVelocity   = rb->iWorldInertiaTensor * rb->m_angMomentum;TestNotValidNum_Physic(rb->m_angVelocity);TestNotValidNum_Physic(rb->m_angVelocity);}// frictionif(tangent.Length() < _EPSILON)continue;tangent.Normalize();//重新计算vel = (Cross(m_angVelocity, r0) + m_lineVelocity) - (Cross(rb->m_angVelocity, r1) + rb->m_lineVelocity);//重新计算切向速度?float tangent_vel = tangent.Dot(vel);if(tangent_vel > -_EPSILON)continue;//摩擦分子float friction_numerator = -tangent_vel * m_fricFac;//摩擦分母float friction_denominator;if(m_mass==StaticMass){friction_denominator = 1.0f / rb->m_mass + tangent.Dot(Cross(rb->iWorldInertiaTensor * Cross(r1, tangent), r1));}else if(rb->m_mass==StaticMass){friction_denominator = 1.0f / m_mass + tangent.Dot(Cross(iWorldInertiaTensor * Cross(r0, tangent), r0));}else{friction_denominator = 1.0f / m_mass + 1.0f / rb->m_mass +tangent.Dot(Cross(iWorldInertiaTensor * Cross(r0, tangent), r0)) +tangent.Dot(Cross(rb->iWorldInertiaTensor * Cross(r1, tangent), r1));}impulse = tangent * friction_numerator / friction_denominator;if(!m_frozen && m_mass!=StaticMass){//AddImpulse(c->point,impulse);m_lineVelocity += impulse / m_mass;m_angMomentum  += Cross(r0, impulse);m_angVelocity   = iWorldInertiaTensor * m_angMomentum;TestNotValidNum_Physic(m_angVelocity);TestNotValidNum_Physic(m_angVelocity);}if(!rb->m_frozen && rb->m_mass!=StaticMass){//rb->AddImpulse(c->point,-impulse);rb->m_lineVelocity -= impulse / rb->m_mass;rb->m_angMomentum  -= Cross(r1, impulse);rb->m_angVelocity   = rb->iWorldInertiaTensor * rb->m_angMomentum;TestNotValidNum_Physic(rb->m_angVelocity);TestNotValidNum_Physic(rb->m_angVelocity);}}}return done;
}int RigidBody::GetJointNum()
{return m_jointNum;
}bool RigidBody::HasJoint(RigidBody* otherRb)
{if (otherRb==NULL){return false;}RigidBody** rb = m_joinedRigidbodies;for(int k = 0; k < m_jointNum; k++,rb++){if(*rb == otherRb) return true;}return false;
}
void RigidBody::RemoveJoint(Joint *joint)
{int index = 0;for(int i = 0; i < m_jointNum; i++) {if(m_joints[i] == joint){break;}index++;}if (index<m_jointNum){//前移for(int i = index; i < m_jointNum-1; i++) {m_joints[i] = m_joints[i+1];m_joinedRigidbodies[i] = m_joinedRigidbodies[i+1];}m_jointNum--;}
}void RigidBody::EditRender()
{if (m_collide){m_collide->EditRender();}
}const vec3 &RigidBody::getWMin(int s)
{return m_collide->m_phyMeshW->getMin(s);
}const vec3 &RigidBody::getWMax(int s)
{return m_collide->m_phyMeshW->getMax(s);
}const vec3 & RigidBody::GetWCenter(int s)
{//todo todo otherRb 碰撞类型是球形时简化updatesurfaceW？？return m_collide->m_phyMeshW->getCenter(s);
}float RigidBody::getRadius(int s)
{return m_phyMesh->getRadius(s);
}
float RigidBody::getVolume(int s)
{return m_phyMesh->getVolume(s);
}PhyObject* RigidBody::GetOwnerObj()
{return m_owner;
}void RigidBody::SetOwnerObj(PhyObject* owner)
{m_owner = owner;
}}

定义关节

//========================================================
//  @Date:     2016.05
//  @File:     Include/Physic/RigidBody.h
//  @Brief:     RigidBody
//  @Author:     LouLei
//  @Email:  twopointfive@163.com
//  @Copyright (Crapell) - All Rights Reserved
//========================================================#pragma once#ifndef __JOINT_H__
#define __JOINT_H__#include "Math/MathLib.h"#define JOINT_DIST 10.0f
namespace Physic
{class PhysicWorld;
class RigidBody;//刚体之间的连接
class Joint
{friend class PhysicWorld;
public:Joint(RigidBody *rigidbodyA, RigidBody *rigidbodyB);virtual ~Joint();virtual void EditRender();protected://链接响应virtual int  Response(float ifps);int RestrictResponse(float ifps, const vec3 &pointA, const vec3 &pointB, float minDist);RigidBody  *m_rigidbodyA;RigidBody  *m_rigidbodyB;PhysicWorld*m_phyWorld;int  ID;
};//球链约束：约束力通过接触点和球心。
//球窝关节：点对点约束，两个刚体的局部枢纽点在世界空间中始终重合，point2pointconstraint//注意限制角是无向的，最大180，限制角 = 180-最大旋转角/2；
//比如限制角=170时，可活动角在+—10即可以最大张开20
//比如限制角=120时，可活动角在+—60即可以最大张开120
//比如限制角=145时，可活动角在+—45即可以最大张开90
//比如限制角=150时，可活动角在+—30即可以最大张开60
//锥扭约束：点对点约束添加了圆锥限制和扭曲限制，比如人体胳膊。
class JointBall : public Joint
{
public:JointBall(RigidBody *rigidbodyA, RigidBody *rigidbodyB, const vec3 &point, const vec3 &restrictAxisA = vec3(1, 0, 0), const vec3 &restrictAxisB = vec3(1, 0, 0), float restrictAng = 180.0f);
//protected:virtual ~JointBall();virtual void EditRender();protected:virtual int  Response(float ifps);//两个刚体链接点的局部坐标系位置vec3  m_pointA;vec3  m_pointB;//局部系下原始限制轴上的点vec3  m_restrictPointA;vec3  m_restrictPointB;//旋转到达最大角时两个限制点间的距离，通过反向两个点的最小距离限制张开角度（？存在伞翻框问题）float m_restrictMinDist;float m_restrictAng;
};//面约束：约束力方向为面的法线方向。
//
//链条约束：约束力方向沿着链条背离物体。//铰链、轴承约束：约束力方向垂直于轴线并通过轴心。只能绕轴旋转，限制了另外两个角的自由度。
//比如做平面单摆、门、车轮
//滚动摩擦？前驱车后车轮有两个方向自由轴？Universal
class JointHinge : public Joint
{
public://限制轴为可旋转角度的角分线JointHinge(RigidBody *rigidbodyA, RigidBody *rigidbodyB, const vec3 &point, const vec3 &hingeAxis, const vec3 &restrictAxisA = vec3(1, 0, 0), const vec3 &restrictAxisB = vec3(1, 0, 0), float restrictAng = 180.0f);virtual ~JointHinge();virtual void EditRender();//设置local旋转轴void SetAxis0(const vec3 &axis);void SetAxis1(const vec3 &axis);void SetAngularVelocity0(float velocity);void SetAngularVelocity1(float velocity);protected:virtual int Response(float ifps);vec3  m_point0A;vec3  m_point0B;vec3  m_point1A;vec3  m_point1B;vec3  m_point;//世界坐标系下的轴 不一定是重合的 比如车轮拐弯时vec3  m_axisA;vec3  m_axisB;mat4  itransformA;mat4  itransformB;//局部系下原始限制轴上的点vec3  m_restrictPointA;vec3  m_restrictPointB;//旋转到达最大角时两个限制点间的距离，通过反向两个点的最小距离限制张开角度（？存在伞翻框问题）float m_restrictMinDist;float m_restrictAng;
};//滑动约束：只能绕轴旋转，和滑动。
class JointSlider : public Joint
{
public:
};//弹簧（橡皮筋）约束：两个刚体可能不接触，橡皮筋无法参与物理碰撞？
class JointSpring: public Joint
{
public:
};//通用6自由度约束：可以模拟其它多种约束。
//前3个自由度是平移，后3个自由度是旋转。每个自由度有3种状态锁死locked自由free限制limited。
class JointUniversal : public Joint
{
public://简化版本 可以绕两个轴旋转 第三个轴锁定( 等同hinge 只不过hingeAxis不重合 为垂直的两个轴)JointUniversal(RigidBody *rigidbodyA, RigidBody *rigidbodyB, const vec3 &point, const vec3 &axisA, const vec3 &axisB, const vec3 &restrictAxisA = vec3(1, 0, 0), const vec3 &restrictAxisB = vec3(1, 0, 0), float restrictAng = 180.0f);virtual ~JointUniversal();virtual void EditRender();protected:virtual int  Response(float ifps);vec3  m_point0A;vec3  m_point0B;vec3  m_point1A;vec3  m_point1B;vec3  m_axisA;vec3  m_axisB;//局部系下原始限制轴上的点vec3  m_restrictPointA;vec3  m_restrictPointB;//旋转到达最大角时两个限制点间的距离，通过反向两个点的最小距离限制张开角度（？存在伞翻框问题）float m_restrictMinDist;float m_restrictAng;
};
}
#endif

布娃娃系统

//========================================================
//  @Date:     2016.05
//  @File:     Include/Physic/RigidBody.h
//  @Brief:     RigidBody
//  @Author:     LouLei
//  @Email:  twopointfive@163.com
//  @Copyright (Crapell) - All Rights Reserved
//========================================================#pragma once#ifndef __PhyObjDoll_H__
#define __PhyObjDoll_H__#include "Math/MathLib.h"namespace RendSys
{class MovieClip;class MC_Skeleton;class Skeleton;
};namespace Physic
{class PhyObjMesh;
class RigidBody;
class PhySkinMesh;
class Joint;//布娃娃系统
//死亡之前 bone驱动rigidbody，死亡之后顺地躺由rigidbody驱动bone。
//角色出现瞬移时,刚体也会乱飞? joint限制太弱？
//角色受到攻击时,根据被攻击的位置和力度动态地做出不同的反应.
// 简单模拟：受到攻击时,让相关骨骼向后做一点旋转,
// 复杂模拟：做一个doll不受重力影响，只受到骨骼动画回复力和物理世界碰撞力的影响
class PhyObjDoll //: public PhyObject
{
public:PhyObjDoll(PhysicWorld* phyWorld);virtual ~PhyObjDoll();enum {head,body,lhand_0,lhand_1,lhand_2,rhand_0,rhand_1,rhand_2,lleg_0,lleg_1,lleg_2,rleg_0,rleg_1,rleg_2,};enum ControlType{PlayerControl,      //玩家控制AnimControl,        //动画控制PhysicControl,      //物理控制Anim2PhysicControl, //由物理控制转到动画控制，物理控制期间将头部向上拉起至脚脱离地面时交由动画系统控制AnimUnionPhysicControl, //受到物理系统碰撞时刚体反应会叠加到原有的骨骼动画上，以模拟实时击打。};//virtual void  OnAdded();//virtual void  OnRemove();virtual void Init(const char* file);virtual void Render();virtual void Update(float time); virtual void SetControlType(ControlType controlType);virtual void SetPos(const vec3& pos);virtual void SetMat(const mat4 &mat);//bool GetBoneMatrix(const char* boneName,vec3* outPos,mat4* outMat);bool GetBoneMatrix(int boneIndex,vec3* outPos,mat4* outMat);RendSys::MovieClip*   m_movieAll;RendSys::MC_Skeleton* m_movieSkin;RendSys::Skeleton*    m_skeleton;RendSys::Skeleton*    m_skeletonRun;class BodyPart{public:char rigidName[64];char boneName[64];RigidBody* rigidBody;float      mass;int        boneIndex;mat4       offsetMat;mat4       invOffsetMat;mat4       animOffsetMat;mat4       invAnimOffsetMat;};int         m_partNum;BodyPart*   m_bodyParts;int         m_jointNum;Joint**     m_joints;ControlType m_controlType;//鞍座位置骨骼id，lookat的tarpos，以及人物骑乘点int         m_saddleIndex;#define   FaceAxis vec3(0,0,1)
//#define   FaceAxis vec3(0,0,-1)PhysicWorld*   m_phyWorld;
};//龙形物体可以根据数学曲线自动生成动画配合按键操作
class PhyObjDragon:public PhyObjDoll
{
public:PhyObjDragon(PhysicWorld* phyWorld);~PhyObjDragon();virtual void Init(const char* file);virtual void Free();virtual void Render();virtual void Update(float time);virtual void SetControlType(ControlType controlType);void EditRender();//设置头部骨骼 头部骨骼局部坐标系为其后骨骼位置计算的基准void SetHeadLocal(const vec3& pos,const vec3& front,float roll);//设置全部骨骼void SetAllBoneLocal (const vec3& pos,const vec3& front,float roll);//两个骨骼之间的距离，等距float m_boneDist;   float m_accumTime;float m_updateTime;class BoneLocal{public://中心线： todo 分离 已经和bone不是一一对应的关系vec3  cenPos;    //局部中心点位置的（世界坐标系）vec3  cenFront;  //中心点面向mat4  cenRot;float cenRoll;//偏移线vec3  bonePos;vec3  boneFront; //骨骼面向方向mat4  boneRot;vec3  lastBonePos;};bool  m_lastBonePosValid;//比如有8个骨骼，每个骨骼使用相对头部骨骼局部坐标系的三角曲线加相移来计算位置。 相对的是8个不同的头部骨骼局部坐标系，这8个坐标系向飘带一样瞬移具有滞后性。BoneLocal*  m_boneLocals;
};//绳索飞爪
class PhyObjRope
{
public:enum ControlType{Throw,      //扔出Swing,      //摆动Pull,       //拉回};class Point{public:Point();void ApplyForce(const vec3& force);void ClearForce();void Simulate(float dt);float mass;                       vec3  pos;                  vec3  vel;                              vec3  force;                                };class Spring                                      {public:void Set(Point* mass1, Point* mass2, float springRat, float springLen, float friction);void Solve();Point* point1;        // 质点1Point* point2;      // 质点2float springRat;    //弹性系数float springLen;    //弹簧长度 float friction;     //摩擦系数};PhyObjRope(PhysicWorld* phyWorld);~PhyObjRope();virtual void Init(const char* file);virtual void Free();virtual void Render();virtual void Update(float time);void  EditRender();void  Solve(float dt);void  SetAll(const vec3& pos);void  BeginThrow(const vec3& pos,const vec3& speed);void  BeginSwing(float len,float speed);void  BeginPull(const vec3& pos,float speed);void  SetControlType(ControlType controlType);Point* GetPoint(int index);int     m_pointNum;                               Point*  m_points;Spring* m_springs; float   m_length;         //绳子长度float   groundRepulsion;    //地面反作用力float   groundFriction;     //地面摩擦float   groundAbsorption; //地面缓冲力float   groundHeight;        //地面高度vec3    gravity;          //重力float   airFriction;        //空气摩擦vec3    m_throwPos;vec3    m_throwSpeed;float   m_swingRadius;float   m_pullSpeed;ControlType m_controlType;PhysicWorld*   m_phyWorld;
};}
#endif

#include "General/Pch.h"
#include "General/File.h"
#include "Physic/PhyMesh.h"
#include "Physic/PhyObject.h"
#include "Physic/PhyObjMesh.h"
#include "Physic/RigidBody.h"
#include "Physic/Joint.h"
#include "Physic/PhyObjDoll.h"
#include "Physic/PhysicWorld.h"
#include "Render/MC_Misc.h"
#include "General/Pce.h"
#include "Render/RendDriver.h"
#include "General/Timer.h"
#include "Render/Camera.h"namespace Physic
{
PhyObjDoll::PhyObjDoll(PhysicWorld* phyWorld)
:m_phyWorld(phyWorld)
,m_movieAll(NULL)
,m_skeleton(NULL)
,m_skeletonRun(NULL)
,m_controlType(PhysicControl)
,m_partNum(0)
,m_bodyParts(NULL)
,m_jointNum(0)
,m_joints(NULL)
{}PhyObjDoll::~PhyObjDoll()
{for (int i=0;i<m_partNum;i++){//SafeDelete(m_bodyParts[i].rigidBody->m_phyMesh);SafeDelete(m_bodyParts[i].rigidBody);}SafeDeleteArray(m_bodyParts);m_partNum = 0;for (int i=0;i<m_jointNum;i++){SafeDelete(m_joints[i]);}SafeDeleteArray(m_joints);m_jointNum = 0;SafeDelete(m_movieAll);SafeDelete(m_skeleton);SafeDelete(m_skeletonRun);
}void PhyObjDoll::Init(const char* physicFile)
{m_skeletonRun = new RendSys::Skeleton;if (m_skeletonRun->LoadFromFile("data/physic/ragdoll_run.bone")==false){//Assert(0,"physic: no physic skeleton");//return;}//一般骨骼动画系统中，子骨骼的原点在和父骨骼的连接处//但是布娃娃系统中要求子骨骼碰撞体的原点在质心 所以需要额外的offsetmat来平衡File file;if (file.Fopen(physicFile,"rt")){ char buf[256];file.ReadString();//dscfile.ReadString(buf,256);m_movieAll = new RendSys::MovieClip;RendSys::LoadConfig cfg(RendSys::LoadConfig::DonotGenAABBTree,true,true);if(m_movieAll->LoadFromFile(buf,&cfg)==false){Assert(0,"physic: no physic movie");return;}m_movieAll->SetFrustumSkipEnable(false,Recursive);m_movieAll->Advance();//读取skinfile.ReadString(buf,256);m_movieSkin = dynamic_cast<MC_Skeleton*>(m_movieAll->GetMovieClip(buf));if (m_movieSkin==NULL){Assert(0,"physic: no skin movie");return;}//读取动作//？todo skin骨架已经确定，无需加载动作file.ReadString(buf,256);m_skeleton = new RendSys::Skeleton;if (m_skeleton->LoadFromFile(buf)==false){Assert(0,"physic: no physic skeleton");return;}m_movieSkin->AssignSkeleton(m_skeleton);//禁止动画更新m_movieSkin->GetMixSkeleton()->m_bFrameAdvance = false;//读取骨骼刚体m_partNum = file.ReadInt();m_bodyParts = new BodyPart[m_partNum];RendSys::MC_Frame* rigidMovies;RendSys::Frame rigidFrames;for (int i=0;i<m_partNum;i++){file.ReadString(m_bodyParts[i].rigidName,64); //rigid namefile.ReadString(m_bodyParts[i].boneName,64);//bone namem_bodyParts[i].boneIndex = m_skeleton->GetBoneIndex(m_bodyParts[i].boneName);if (m_bodyParts[i].boneIndex<0){Assert(0,"physic: no bone");return;}file.ReadBlock(buf,256);//碰撞类型int collideType = 0;if (strcmp(buf,"collidemesh")==0){collideType = RigidBody::CollideMesh;}else if (strcmp(buf,"collidesphere")==0){collideType = RigidBody::CollideSphere;}else{Assert(0,"physic: no collide style");}//惯量类型file.ReadBlock(buf,256);int bodyType = 0;if (strcmp(buf,"bodybox")==0){bodyType |= RigidBody::BodyBox;}else if (strcmp(buf,"bodysphere")==0){bodyType |= RigidBody::BodySphere;}else if (strcmp(buf,"bodycylinder")==0){bodyType |= RigidBody::BodyCylinder;}else{Assert(0,"physic: no body style");}float mass = file.ReadFloat(); m_bodyParts[i].mass = mass;float ringFac = file.ReadFloat(); float fricFac = file.ReadFloat(); file.ReadString();//{rigidMovies = (RendSys::MC_Frame*)m_movieAll->GetMovieClip(m_bodyParts[i].rigidName); rigidFrames  = rigidMovies->GetFrameLine()->m_keyFrames[0];PhyMesh* phyMesh = new PhyMesh(rigidMovies,1);//rigidMovies->ZeroFrames();if (collideType==0) //ball 半径 密度{m_bodyParts[i].rigidBody = new RigidBody(phyMesh, mass, ringFac, fricFac, RigidBody::CollideSphere | RigidBody::BodySphere);}else //mesh{m_bodyParts[i].rigidBody = new RigidBody(phyMesh, mass, ringFac, fricFac, RigidBody::CollideMesh | RigidBody::BodyBox);}//不管质量是否为0都用第一帧的位置初始{rigidFrames.CalQuatMatrix();m_bodyParts[i].rigidBody->m_resetPos = rigidFrames.m_pos;m_bodyParts[i].rigidBody->m_resetRot = rigidFrames.m_rot*DEG2RAD;m_bodyParts[i].rigidBody->SetMat(rigidFrames.m_matrix);m_bodyParts[i].offsetMat = rigidFrames.m_matrix;m_bodyParts[i].invOffsetMat = rigidFrames.m_matrix;m_bodyParts[i].invOffsetMat.Inverse();m_bodyParts[i].animOffsetMat = m_skeleton->m_bones[m_bodyParts[i].boneIndex].m_frameLine.m_keyFrames[0].m_matrix;m_bodyParts[i].invAnimOffsetMat = m_bodyParts[i].animOffsetMat;m_bodyParts[i].invAnimOffsetMat.Inverse();}m_phyWorld->AddRigidBody(m_bodyParts[i].rigidBody);}}//读取连接//joint要排在rigidbody之后 否则可能查找不到m_jointNum = file.ReadInt();if(m_jointNum>0)m_joints = new Joint*[m_jointNum];for (int i=0;i<m_jointNum;i++){String name = file.ReadString();file.ReadBlock(buf,256);//连接类型int jointType = 0;vec3 axis[4];float restriction_angle;if (strcmp(buf,"ball")==0){jointType = 1;}else if (strcmp(buf,"hinge")==0){jointType = 2;file.ReadFloatArray(&axis[0].x,3);file.ReadFloatArray(&axis[1].x,3);file.ReadFloatArray(&axis[2].x,3);restriction_angle = file.ReadFloat();}else if (strcmp(buf,"universal")==0){jointType = 3;file.ReadFloatArray(&axis[0].x,3);file.ReadFloatArray(&axis[1].x,3);file.ReadFloatArray(&axis[2].x,3);file.ReadFloatArray(&axis[3].x,3);restriction_angle = file.ReadFloat();}else{Assert(0,"physic: no joint style");} {//辅助轴moviefor (int j=0;j<4;j++){sprintf(buf,"%s_axis%d",name.c_str(),j);MC_Frame* axisMovie = (RendSys::MC_Frame*)m_movieAll->GetMovieClip(buf); if (axisMovie==NULL){break;}RendSys::Frame axisFrame = axisMovie->GetFrameLine()->m_keyFrames[0];axisFrame.CalQuatMatrix();axis[j] = axisFrame.m_matrix.RotationPart()*vec3(0,1,0);}}String objName0 = file.ReadString(); String objName1 = file.ReadString(); {MC_Frame* jointClip = dynamic_cast<MC_Frame*>(m_movieAll->GetMovieClip(name.c_str()));if (jointClip==NULL){Assert(0,"physic: no joint obj");continue;//return;}RigidBody* rb0 = NULL;RigidBody* rb1 = NULL;for (int j=0;j<m_partNum;j++){if (objName0==m_bodyParts[j].rigidName){rb0 = m_bodyParts[j].rigidBody;break;}}for (int j=0;j<m_partNum;j++){if (objName1==m_bodyParts[j].rigidName){rb1 = m_bodyParts[j].rigidBody;break;}}if (rb0&&rb1){Joint* joint = NULL;vec3 pos = jointClip->GetFrameLine()->m_keyFrames[0].m_pos*UnitScale;if(jointType==1){joint = new JointBall(rb0, rb1, pos);}else if(jointType==2){//continue;joint = new JointHinge(rb0, rb1,pos,axis[0],axis[1],axis[2],restriction_angle);}else if(jointType==3){//continue;joint = new JointUniversal(rb0, rb1,pos,axis[0],axis[1],axis[2],axis[3],restriction_angle);}m_joints[i] = joint;m_phyWorld->AddJoint(joint);}else{int a = 0;}}}//end jointm_saddleIndex = 0;  //默认 head}
}void PhyObjDoll::SetPos(const vec3& pos)
{}void PhyObjDoll::SetMat(const mat4 &m)
{//if (m_movieSkin)//{//    if (m_controlType==PhysicControl//        ||m_controlType==PlayerControl)// {//     //根据刚体更新骨骼//        for (int i=0;i<m_partNum;i++)//       {//         if(m_bodyParts[i].boneIndex>=0)//           {//             m_bodyParts[i].rigidBody->SetMat(m);//           }//     }// }// else if (m_controlType==AnimControl)//    {//     //根据骨骼更新刚体//        Frame frame;//      frame.m_matrix = m;//      m_movieSkin->SetProgramFrame(frame);//       m_movieSkin->Advance(-1);//  }// else if (m_controlType==AnimUnionPhysicControl)// {//     //物理叠加骨骼动画//        for (int i=0;i<m_partNum;i++)//       {//         if(m_bodyParts[i].boneIndex>=0)//           {//             m_bodyParts[i].rigidBody->SetMat(m);//           }//     }//     Frame frame;//      frame.m_matrix = m;//      m_movieSkin->SetProgramFrame(frame);//       m_movieSkin->Advance(-1);//  }//}
}void PhyObjDoll::SetControlType(ControlType controlType)
{if (m_controlType==controlType){return;}m_controlType = controlType;if (controlType==AnimControl){Frame frame;frame.m_pos = m_bodyParts[0].rigidBody->m_massCenter;//todo body rigid bodyframe.CalQuatMatrix();m_movieSkin->SetProgramFrame(&frame);m_movieSkin->AssignSkeleton(m_skeletonRun);for (int i=0;i<m_partNum;i++){m_bodyParts[i].rigidBody->m_mass = StaticMass;}}else if (controlType==PhysicControl){//清除program frameFrame frame;frame.CalQuatMatrix();m_movieSkin->SetProgramFrame(&frame);m_movieSkin->AssignSkeleton(m_skeleton);for (int i=0;i<m_partNum;i++){m_bodyParts[i].rigidBody->m_mass = m_bodyParts[i].mass;}}else if (controlType==PlayerControl){Frame frame;frame.CalQuatMatrix();m_movieSkin->SetProgramFrame(&frame);m_movieSkin->AssignSkeleton(m_skeleton);for (int i=0;i<m_partNum;i++){m_bodyParts[i].rigidBody->m_mass = StaticMass;}}else if (controlType==AnimUnionPhysicControl){Frame frame;frame.m_pos = m_bodyParts[0].rigidBody->m_massCenter;//todo body rigid bodyframe.CalQuatMatrix();m_movieSkin->SetProgramFrame(&frame);m_movieSkin->AssignSkeleton(m_skeletonRun);for (int i=0;i<m_partNum;i++){m_bodyParts[i].rigidBody->m_mass = m_bodyParts[i].mass;}}
}void PhyObjDoll::Render()
{if (m_movieSkin){m_movieSkin->RendClip();}
}void PhyObjDoll::Update(float time)
{PROFILEFUN_Physic("PhysicDoll::Update();",0.0f,ALWAYSHIDE);if (m_movieSkin){if (m_controlType==PhysicControl||m_controlType==PlayerControl){//根据刚体更新骨骼m_movieSkin->GetMixSkeleton()->m_bFrameAdvance = false;for (int i=0;i<m_partNum;i++){if(m_bodyParts[i].boneIndex>=0){//program frame 已经被清除//在max中，rigidbody经过matOffset变换后和bone经过matFrame变换后重合//在physic中，rigidbody经过matTransform变换后从原坐标系变换到新系//此时要使得bone仍然和rigidbody重合需要的变换矩阵为先invOffsetMat再matTransformmat4 mat = m_bodyParts[i].rigidBody->transform * m_bodyParts[i].invOffsetMat;//无需附加矩阵m_movieSkin->GetMixSkeleton()->TransMixBone(m_bodyParts[i].boneIndex,mat);//mat = mat * m_bodyParts[i].animOffsetMat;//m_movieSkin->GetMixSkeleton()->SetMixBone(m_bodyParts[i].boneIndex,mat);}}}else if (m_controlType==AnimControl){//根据骨骼更新刚体m_movieSkin->GetMixSkeleton()->m_bFrameAdvance = true;for (int i=0;i<m_partNum;i++){if(m_bodyParts[i].boneIndex>=0){//program frame 没有被清除m_bodyParts[i].rigidBody->SetMat(m_movieSkin->GetProgramFrame()->m_matrix*m_movieSkin->GetMixSkeleton()->m_mixFrameArr[m_bodyParts[i].boneIndex].m_matrix*m_bodyParts[i].invAnimOffsetMat*m_bodyParts[i].offsetMat);}}}else if (m_controlType==AnimUnionPhysicControl){//物理叠加骨骼动画//开启骨骼动画m_movieSkin->GetMixSkeleton()->m_bFrameAdvance = true;//m_movieSkin->Advance(0.0f); //更新纯动画matrixm_movieSkin->Advance(time); //更新纯动画matrixfloat lerp = 0.8f;//lerp *= G_Timer->GetStepTimeLimited();Clamp(lerp,0.3f,0.9f);for (int i=0;i<m_partNum;i++){if(m_bodyParts[i].boneIndex>=0){//program frame 没有被清除//计算动画matrix对应的刚体matrixmat4 matRigidFromAnim = m_movieSkin->GetProgramFrame()->m_matrix*m_movieSkin->GetMixSkeleton()->m_mixFrameArr[m_bodyParts[i].boneIndex].m_matrix*m_bodyParts[i].invAnimOffsetMat*m_bodyParts[i].offsetMat;//混合刚体matrix和‘动画matrix’mat4 matRigidMix;matRigidMix.Slerp(matRigidFromAnim,m_bodyParts[i].rigidBody->transform,lerp);m_bodyParts[i].rigidBody->SetMat(matRigidMix);mat4 mat = matRigidMix * m_bodyParts[i].invOffsetMat;//相对于动画第一帧的矩阵,实际动画已经不是第一帧了mat = mat * m_bodyParts[i].animOffsetMat;            //使用动画第一帧的matrix//变换到programframe的local局部坐标系mat4 invProgramFrame = m_movieSkin->GetProgramFrame()->m_matrix;invProgramFrame.Inverse();mat = invProgramFrame * mat;//设置到动画系统 //注意有可能要附加矩阵m_movieSkin->GetMixSkeleton()->SetMixBone(m_bodyParts[i].boneIndex,mat);}}//禁用骨骼动画m_movieSkin->GetMixSkeleton()->m_bFrameAdvance = false;}m_movieSkin->Advance(-1);}
}bool PhyObjDoll::GetBoneMatrix(int boneIndex,vec3* outPos,mat4* outMat)
{if (boneIndex<0 || boneIndex>=m_partNum){return false;}if (outMat){*outMat = m_bodyParts[boneIndex].rigidBody->transform;// * m_bodyParts[boneIndex].invOffsetMat;if (outPos){*outPos = outMat->GetTranslate();}}else{mat4 mat = m_bodyParts[boneIndex].rigidBody->transform;// * m_bodyParts[boneIndex].invOffsetMat;if (outPos){*outPos = outMat->GetTranslate();}}return true;
}//==================^_^==================^_^==================^_^==================^_^
PhyObjDragon::PhyObjDragon(PhysicWorld* phyWorld)
:PhyObjDoll(phyWorld)
,m_boneDist(0)
,m_accumTime(0)
,m_updateTime(0)
,m_boneLocals(NULL)
,m_lastBonePosValid(false)
{
}PhyObjDragon::~PhyObjDragon()
{Free();
}
void PhyObjDragon::Init(const char* physicFile)
{PhyObjDoll::Init(physicFile);m_saddleIndex = int(m_partNum*0.2f);//连接//safedelete m_jointsm_jointNum = m_partNum-1;//19;m_joints = new Joint*[m_jointNum];for (int i=0;i<m_jointNum;i++){vec3 axis[4];RigidBody* rb0 = m_bodyParts[i].rigidBody;RigidBody* rb1 = m_bodyParts[i+1].rigidBody;mat3 mat = rb1->transform.RotationPart();axis[0] = mat*vec3(1,0,0);axis[1] = mat*vec3(0,1,0);axis[2] = mat*vec3(0,0,1);axis[3] = mat*vec3(0,0,1);Joint* joint = NULL;vec3 pos = rb1->m_massCenter*UnitScale;joint = new JointUniversal(rb0, rb1,pos,axis[0],axis[1],axis[2],axis[3],145);m_joints[i] = joint;//joint = new JointBall(rb0, rb1,pos);m_phyWorld->AddJoint(joint);}if (m_partNum>0)m_boneDist = (m_bodyParts[m_partNum-1].rigidBody->m_massCenter-m_bodyParts[0].rigidBody->m_massCenter).Length()/m_partNum;         m_boneLocals = new BoneLocal[m_partNum];SetAllBoneLocal(m_bodyParts[0].rigidBody->m_massCenter,m_bodyParts[0].rigidBody->m_lineVelocity,0);
}void PhyObjDragon::Free()
{SafeDeleteArray(m_boneLocals);
}void PhyObjDragon::Render()
{PhyObjDoll::Render();
}void PhyObjDragon::EditRender()
{//G_RendDriver->DisableRendState(RS_DEPTH_TEST);G_RendDriver->DisableRendState(RS_TEXTURE_2D);G_RendDriver->DisableRendState(RS_LIGHTING);G_RendDriver->SetLineWidth(1);绘制骨骼updir//G_RendDriver->Color3f(0, 1, 0);  //G_RendDriver->RendBegin(RS_LINES);//for (int i = 0; i < m_partNum - 1; ++i)//{// vec3& v1 = m_bodyParts[i].rigidBody->m_massCenter;//    vec3& v2 = v1 + m_bodyParts[i].rigidBody->transform.RotationPart()*vec3(0,10,0);// G_RendDriver->Vertex3fv(v1);//   G_RendDriver->Vertex3fv(v2);//}//G_RendDriver->RendEnd();//绘制中心连线G_RendDriver->Color3f(1, 0, 0);   G_RendDriver->RendBegin(RS_LINES);for (int i = 0; i < m_partNum - 1; ++i){vec3& v1 = m_boneLocals[i].cenPos;vec3& v2 = m_boneLocals[i+1].cenPos;G_RendDriver->Vertex3fv(v1);G_RendDriver->Vertex3fv(v2);}G_RendDriver->RendEnd();//绘制中心点G_RendDriver->Color3f(0, 0, 1); G_RendDriver->SetPointSize(4);G_RendDriver->RendBegin(RS_POINTS);for (int i = 0; i < m_partNum; ++i){vec3& v1 = m_boneLocals[i].cenPos;G_RendDriver->Color3f( 0.5f*((i+0)%3), 0.5f*((i+1)%3), 0.5f*((i+2)%3));   G_RendDriver->Vertex3fv(v1);}G_RendDriver->RendEnd();G_RendDriver->EnableRendState(RS_LIGHTING);G_RendDriver->EnableRendState(RS_TEXTURE_2D);//G_RendDriver->EnableRendState(RS_DEPTH_TEST);
}void PhyObjDragon::SetHeadLocal(const vec3& pos,const vec3& front_,float roll)
{vec3 front = front_;//pos - m_headLocals[0].pos;front.Normalize();if (front.LengthSq()<0.1f){if (m_boneLocals[0].cenFront.LengthSq()<0.1f){front = FaceAxis;}else{//速度不变front = m_boneLocals[0].cenFront;}}m_boneLocals[0].cenPos = pos;m_boneLocals[0].cenFront = front;如果这里FaceAxis使用vec3(0,0,-1)，那么LookAt也要使用-1，SetAllBoneLocal里也是，SetControlType里也是，BeginLookAt里也不能旋转pi。m_boneLocals[0].cenRot.FromToDir(FaceAxis,front);m_boneLocals[0].cenRoll = roll;//SetAllHeadLocal(pos,front_,up);
}void PhyObjDragon::SetAllBoneLocal(const vec3& pos,const vec3& front_,float roll)
{vec3 front = front_;//pos - m_headLocals[0].pos;front.Normalize();if (front.LengthSq()<0.1f){if (m_boneLocals[0].cenFront.LengthSq()<0.1f){front = FaceAxis;}else{//速度不变front = m_boneLocals[0].cenFront;}}for (int i=0;i<m_partNum;i++){m_boneLocals[i].cenPos = pos+m_boneDist*front*i;m_boneLocals[i].cenFront = front;m_boneLocals[i].cenRoll = roll;m_boneLocals[i].cenRot.FromToDir(FaceAxis,front);}m_lastBonePosValid = false;
}void PhyObjDragon::Update(float time)
{PROFILEFUN_Physic("PhyObjDragon::Update();",0.0f,ALWAYSHIDE);//震荡的原因：update写在了render里，设置位置后又被更新了？PhyObjDoll::Update(time);//todo 拉伸改进 增加骨骼数量 多个骨骼对应一个摆点 或 减少摆点个数 和骨骼独立出来//使用pathdeform的话 碰撞不好处理if (m_controlType==PlayerControl){//计算中心点位置//鼠标移动 震荡加剧？？？？{m_updateTime += time;float SimStepTime = 0.002f;   //float SimStepTime = time/300.0f;vec3 tar;vec3 dir;vec3 newCenter;mat4 mat_;//位置和旋转的插值系数不同while(m_updateTime > 0){//零头float time = min(SimStepTime,m_updateTime);//m_updateTime -= SimStepTime;m_updateTime -= time;float lerpRot = 0.1f*time/0.03f;Clamp(lerpRot,0.00001f,0.99999f);float lerpPos = 0.95f*time/0.03f;Clamp(lerpPos,0.00001f,0.99999f);//lerpPos = 1.0f;float lerpRoll = 0.05f*time/0.03f;Clamp(lerpRoll,0.00001f,0.99999f);//相移BoneLocalfor (int i=1;i<m_partNum;i++){BoneLocal* thisLocal = &m_boneLocals[i];BoneLocal* prevLocal = &m_boneLocals[i-1];//位置和旋转的插值系数不同newCenter = thisLocal->cenPos;//旋转拉近插值dir = prevLocal->cenFront;tar = prevLocal->cenPos - dir*m_boneDist;newCenter.Slerp(newCenter,tar,lerpRot);//直线拉近插值dir = prevLocal->cenPos - newCenter;dir.Normalize();tar = prevLocal->cenPos - dir*m_boneDist;newCenter.Slerp(newCenter,tar,lerpPos);//thisLocal->cenPos = newCenter;thisLocal->cenFront = prevLocal->cenPos-newCenter;thisLocal->cenFront.Normalize();//thisLocal->cenRoll = prevLocal->cenRoll*lerpRoll + thisLocal->cenRoll*(1-lerpRoll);//直线调试震荡//thisLocal->cenFront = prevLocal->cenFront;//thisLocal->cenPos = prevLocal->cenPos+prevLocal->cenFront*m_boneDist;//thisLocal->cenRoll = prevLocal->cenRoll;}}}//构造中心旋转mat4 mat;vec3 parentY,thisY;mat4 twist;for (int i=1;i<m_partNum;i++){BoneLocal* thisLocal = &m_boneLocals[i];BoneLocal* prevLocal = &m_boneLocals[i-1];mat.FromToDir(prevLocal->cenFront,thisLocal->cenFront);thisLocal->cenRot = mat*prevLocal->cenRot;限制扭曲 无效？//parentY = prevLocal->cenRot*vec3(0,1,0);parentY = thisLocal->cenRot*vec3(0,1,0);//thisY   = thisLocal->cenRot*vec3(0,1,0);parentY投影到thisLocal->cenFront的切平面上//parentY = parentY - thisLocal->cenFront*parentY.Dot(thisLocal->cenFront);//float rad = SafeAcos(thisY.Dot(parentY));//rad -= _PI*0.015F;//if (rad > 0)//{//    twist.FromAxisAngle(thisY.Cross(parentY),rad);//    thisLocal->cenRot = twist * thisLocal->cenRot;//}//mat.FromToDir(vec3(0,0,-1),vec3(0,1,0),thisLocal->cenFront,vec3(0,1,0));//FaceAxis//thisLocal->cenRot = mat;}//test 通过曲线函数模拟   参考鱼类游动：头不动 尾部摆动最大 正弦波浪  各段面向具有滞后性m_accumTime += time;//计算骨骼位置{float bodyLen = m_partNum* m_boneDist;//80vec3  bonePos;float saddlePos = 0.2f; //鞍座位置相移(前爪处 约1/5身长)float saddleAmp = 0.01f;//鞍座位置振幅float ampMax    = bodyLen*0.4f; //尾部最大幅度float pMax      = 3.0f;         //尾部最大相移 弧度制float accumPosLen = 0;float curvePos = 0;vec3  lerpCenPos;float boneDistSq = m_boneDist*m_boneDist;vec3  dif;for (int i = 0; i < m_partNum; ++i)       {  //会震荡？因为每次抚平的拐角不同？//根据实际曲线上的点距拉近, 骨骼对应的boneLocal的index不再是i，而是小于i的某浮点值curvePosaccumPosLen = 0;for(int j=0;j<200;j++){if (i < saddlePos+2){curvePos = i;}//mat = m_boneLocals[int(curvePos)].cenRot;mat.Slerp(m_boneLocals[int(curvePos)].cenRot,m_boneLocals[int(curvePos)+1].cenRot,curvePos-int(curvePos));bonePos.z =  0;float p     = float(curvePos)/m_partNum;//[0~1]   //float amp = ampMax;//等幅float amp   = ampMax * (abs(p-saddlePos) + saddleAmp);   //前爪（1/5身长）处幅度最小 为鞍座位置//21*(abs(x/20-0.2) + 0.01);float theta = m_accumTime-p*pMax;//靠近尾部幅度线性增大，横切面等比放大（各横切面线无相对旋转），点在横切线上相移应该相应变小，否则实际距离会变大，导致曲线变的尖锐而产生过渡转折？//过渡转折仅发生在拐弯时？ 横切面沿纵轴没有旋转，但已经不再平行，两个横切面产生相交？//float theta = m_accumTime - p*pMax/(amp/ampMax/saddleAmp)*0.01f;//直线[21~1~81]//float theta = m_accumTime - sqrt(p)*pMax;//float r   = amp;                           //横切面为 圆       //float r   = amp*(sin(2.5*theta));          //横切面为 星形线  float r   = amp*(sin(2.5*sin(2.0*theta))); //横切面为 花瓣曲线 极坐标方程r=sin(2.5*sin(2.0*x));  // 2.5f大瓣数; 2.0f小瓣数bonePos.x =  r*cos(theta);                bonePos.y =  r*sin(theta);                bonePos   =  mat*bonePos;//利用数学曲线编辑器检查 曲线没问题 参见dragon.mathobj//r = 21*(abs(x/20-0.2) + 0.01)* sin(2.5*sin(2.0* (t-x/20*3) )) //x = 21*(abs(s/20-0.2) + 0.01)* sin(2.5*sin(2.0* (t-s/20*3) )) * cos(t-s/20*3)//y = 21*(abs(s/20-0.2) + 0.01)* sin(2.5*sin(2.0* (t-s/20*3) )) * sin(t-s/20*3)  //z = s//s[1~20] //max布线太稀 也不是影响因素？lerpCenPos.Slerp(m_boneLocals[int(curvePos)].cenPos,m_boneLocals[int(curvePos)+1].cenPos,curvePos-int(curvePos));bonePos  += lerpCenPos;if(j>10){//一般不会超过10步，跨中心点没问题，因为后续只是需要中心点的位置int a = 0;}if (i < saddlePos+2){m_boneLocals[i].bonePos = bonePos;break;}else{dif = bonePos - m_boneLocals[i-1].bonePos;if (dif.LengthSq()>boneDistSq //curvePos沿曲线移动，超出骨骼长度时可以放置骨骼了//||accumPosLen>1// 没有大于1的){//m_boneLocals[i].bonePos = bonePos;dif.Normalize();m_boneLocals[i].bonePos = m_boneLocals[i-1].bonePos + dif*m_boneDist;break;}}accumPosLen += 0.1f;curvePos += 0.1f; //步进  减少步长不能避免扭曲  拐角处向外扩张的法线会相交？}}}//平滑骨骼位置 平滑后长度改变 不再衔接 拐弯稍好  float lerpPos = 0.2f*time/0.03f;Clamp(lerpPos,0.00001f,0.99999f);if (m_lastBonePosValid){for (int i=0;i<m_partNum; ++i){BoneLocal* thisLocal = &m_boneLocals[i];thisLocal->bonePos = thisLocal->bonePos*lerpPos + thisLocal->lastBonePos*(1-lerpPos);}}m_lastBonePosValid = true;for (int i=0;i<m_partNum; ++i){BoneLocal* thisLocal = &m_boneLocals[i];thisLocal->lastBonePos = thisLocal->bonePos;}//计算骨骼旋转for (int i = 0; i < m_partNum; ++i)        {BoneLocal* thisLocal = &m_boneLocals[i];BoneLocal* prevLocal = &m_boneLocals[i-1];BoneLocal* nextLocal = &m_boneLocals[i+1];//todo 根据实际曲线上的点距拉近, 对应的boneLocal cenRot 的index不再是i，而是小于i的某值mat = thisLocal->cenRot;//刚体碰撞mesh前粗后细if (i==0){//thisLocal->boneFront = thisLocal->bonePos-m_boneLocals[i+__EPSILON].bonePos;//切线旋转骨骼首尾不衔接thisLocal->boneFront = thisLocal->bonePos - nextLocal->bonePos;thisLocal->boneFront.Normalize();//mat.FromToDir(FaceAxis,vec3(0,1,0),thisLocal->boneFront,vec3(0,1,0));//有问题  应该取反//mat.FromToDir(vec3(0,0,-1),vec3(0,1,0),thisLocal->boneFront,vec3(0,1,0));//碰撞体debug显示 反向  骨骼反向扭曲//龙头updir朝上，然后随按键打滚mat4 matRoll;matRoll.FromAxisAngle(thisLocal->boneFront,thisLocal->cenRoll);mat = matRoll*mat;}else{//if (i<m_partNum-1){thisLocal->boneFront = thisLocal->bonePos - nextLocal->bonePos;//float len = thisLocal->boneFront.Length() - m_boneDist;//if (abs(len)>0.01f)//{//    int a = 0;//}}else{thisLocal->boneFront = prevLocal->boneFront;}thisLocal->boneFront.Normalize();//往回走时有问题//没有适度拧麻花，垂直升降时没问题mat.FromToDir(-prevLocal->boneFront,-thisLocal->boneFront);//roll可以适度瞬移，但垂直升降和直线折返（往后走）时有问题//mat.FromToDir(-prevLocal->boneFront,vec3(0,1,0),-thisLocal->boneFront,vec3(0,1,0));mat = mat * prevLocal->boneRot;限制扭曲 无效？parentY = prevLocal->boneRot*vec3(0,1,0);//parentY = thisLocal->boneRot*vec3(0,1,0);//thisY   = mat*vec3(0,1,0);parentY投影到thisLocal->boneFront的切平面上//parentY = parentY - thisLocal->boneFront*parentY.Dot(thisLocal->boneFront);//float rad = SafeAcos(thisY.Dot(parentY)) - _PI*0.15F;//if (rad > 0)//{//  twist.FromAxisAngle(thisY.Cross(parentY),rad);//    mat = twist * mat;//}}thisLocal->boneRot = mat;}//将骨骼矩阵更新到刚体for (int i = 0; i < m_partNum; ++i)      {mat = m_boneLocals[i].boneRot;//质心位置在骨骼中点//mat.SetTranslate(m_boneLocals[i].bonePos);mat.SetTranslate(m_boneLocals[i].bonePos - m_boneLocals[i].boneFront*m_boneDist*0.5f);m_bodyParts[i].rigidBody->SetMat(mat);}}//放在后面会导致震荡？？//PhysicDoll::Update(time);
}void PhyObjDragon::SetControlType(ControlType controlType)
{PhyObjDoll::SetControlType(controlType);if (m_controlType==PlayerControl){RigidBody* rb = m_bodyParts[m_saddleIndex].rigidBody;SetAllBoneLocal(rb->m_massCenter+vec3(0,10,0),rb->transform.RotationPart()*FaceAxis,0);}
}//==================^_^==================^_^==================^_^==================^_^
void PhyObjRope::Point::Simulate(float dt)
{if (mass==0){return;}vel += (force / mass) * dt;               pos += vel * dt;
}void PhyObjRope::Point::ClearForce()
{force.x = 0;force.y = 0;force.z = 0;
}void PhyObjRope::Point::ApplyForce(const vec3& force)
{this->force += force;
}PhyObjRope::Point::Point()
{}void PhyObjRope::Spring::Solve()
{vec3 dif = point1->pos - point2->pos;                           float distance = dif.Length();                                         vec3 force;                                                             if (distance >_EPSILON)                      {//弹簧力与距离有关dif /= distance;force += dif * (distance - springLen) * (-springRat);//直拉系数dif *= ((distance - springLen)*0.5f*0.1f);if(point1->mass)point1->pos -= dif;                                                  if(point2->mass)point2->pos += dif; }//摩擦力与速度有关force += -(point1->vel - point2->vel) * friction;    //静态质点if (point1->mass == StaticMass){point2->ApplyForce(force*-2);}else if (point2->mass == StaticMass){point1->ApplyForce(force*2);                                                   }else{point1->ApplyForce(force);                                                 point2->ApplyForce(-force);}}void PhyObjRope::Spring::Set(Point* mass1, Point* point2, float springConstant, float springLength, float friction)
{this->springRat = springConstant;          this->springLen = springLength;             this->friction = friction;      this->point1 = mass1;                           this->point2 = point2;
}PhyObjRope::PhyObjRope(PhysicWorld* phyWorld)
:m_phyWorld(phyWorld)
,m_points(NULL)
,m_springs(NULL)
,m_controlType(Pull)
,m_pullSpeed(20)
{
}PhyObjRope::~PhyObjRope()
{Free();
}
void PhyObjRope::Init(const char* physicFile)
{int     pointNum=                 50;                     // 质点数float     mass   =                   0.05f;                  float   springConstant =           500.0f;                 // 弹性系数10000                    float   springFriction =   0.2f;                   // 弹簧的内摩擦力vec3      gravitation=               vec3(0, -9.81f, 0);     // 万有引力float    airFriction=       0.02f;                  // 空气摩擦力float   groundRepulsion=   100.0f;                 // 地面反作用系数float     groundFriction =   0.2f;                   // 地面摩擦系数float  groundAbsorption=  2.0f;                   // 地面缓冲系数float  groundHeight            =  -1.5f;                  // 地面高度;m_length=              100;float   springLen =            m_length/(pointNum-1);                  //m_pointNum = pointNum;m_points = new Point[pointNum];           for (int i = 0; i < pointNum; ++i)    {m_points[i].mass = mass;}m_points[0].mass = 0;m_points[pointNum-1].mass = 1.0f;this->gravity = gravitation;this->airFriction = airFriction;this->groundFriction = groundFriction;this->groundRepulsion = groundRepulsion;this->groundAbsorption = groundAbsorption;this->groundHeight = groundHeight;for (int i = 0; i < pointNum; ++i)           // 设置质点位置{m_points[i].pos.x = i * springLen;       m_points[i].pos.y = 0;                     m_points[i].pos.z = 0;                     }m_springs = new Spring[pointNum - 1];         //创建弹簧for (int i = 0; i < pointNum - 1; ++i)          {m_springs[i].Set(&m_points[i], &m_points[i + 1], springConstant, springLen, springFriction);}
}void PhyObjRope::Free()
{delete[](m_points);m_points = NULL;delete[](m_springs);m_springs = NULL;
}void PhyObjRope::Render()
{EditRender();
}void PhyObjRope::EditRender()
{//G_RendDriver->DisableRendState(RS_DEPTH_TEST);G_RendDriver->DisableRendState(RS_TEXTURE_2D);G_RendDriver->DisableRendState(RS_LIGHTING);Shadow//G_RendDriver->Color3f(0, 0, 0);                                                  //for (int i = 0; i < m_pointNum - 1; ++i)//{//   Point* mass1 = GetPoint(i);//  vec3* v1 = &mass1->pos;//   Point* point2 = GetPoint(i + 1);//    vec3* v2 = &point2->pos;//  G_RendDriver->SetLineWidth(1);// G_RendDriver->RendBegin(RS_LINES);// G_RendDriver->Vertex3f(v1->x, groundHeight, v1->z);    //  G_RendDriver->Vertex3f(v2->x, groundHeight, v2->z);        //  G_RendDriver->RendEnd();//}//Drawing Rope.G_RendDriver->Color3f(1, 0, 0); G_RendDriver->SetLineWidth(1);G_RendDriver->RendBegin(RS_LINES);for (int i = 0; i < m_pointNum - 1; ++i){Point* point1 = GetPoint(i);Point* point2 = GetPoint(i + 1);G_RendDriver->Vertex3fv(point1->pos);G_RendDriver->Vertex3fv(point2->pos);}G_RendDriver->RendEnd();G_RendDriver->Color3f(0, 0, 1);    G_RendDriver->SetPointSize(4);G_RendDriver->RendBegin(RS_POINTS);for (int i = 0; i < m_pointNum; ++i){Point* mass1 = GetPoint(i);G_RendDriver->Vertex3fv(mass1->pos);}G_RendDriver->RendEnd();G_RendDriver->EnableRendState(RS_LIGHTING);G_RendDriver->EnableRendState(RS_TEXTURE_2D);//G_RendDriver->EnableRendState(RS_DEPTH_TEST);
}void PhyObjRope::SetAll(const vec3& pos)
{vec3 zero;for (int i = 0; i < m_pointNum; ++i)           {m_points[i].pos = pos;    m_points[i].vel = zero;m_points[i].force = zero;}
}void PhyObjRope::Update(float time)
{PROFILEFUN_Physic("PhyObjRope::Update();",0.0f,ALWAYSHIDE);//int stepNum = (time / 0.002f) + 1;                    float dt = time / stepNum;                                         for (int i = 0; i < stepNum; ++i) {if (m_controlType==Throw){m_points[0].mass = 0;m_points[0].vel = m_throwSpeed;vec3 newpos = m_points[0].pos + dt*m_throwSpeed;if (Dot(newpos-m_throwPos,m_points[0].pos-m_throwPos)<=_EPSILON){m_points[0].pos = m_throwPos;}else{m_points[0].pos = newpos;}m_points[m_pointNum-1].mass = 0;m_points[m_pointNum-1].vel = vec3();//m_points[m_pointNum-1].pos += dt*m_throwSpeed;}else if (m_controlType==Swing){m_points[0].mass = 0;m_points[0].vel = vec3();//m_points[0].pos += dt*m_throwSpeed;m_points[m_pointNum-1].mass = 10.0f;//m_points[m_pointNum-1].vel = vec3();//m_points[m_pointNum-1].pos += dt*m_throwSpeed;float newlen = m_length - m_pullSpeed*dt;if (newlen < m_swingRadius){m_length = m_swingRadius;}else{m_length = newlen;float springLength = m_length/(m_pointNum-1);for (int i = 0; i < m_pointNum - 1; ++i)            {m_springs[i].springLen = springLength;}}}else if (m_controlType==Pull){m_points[0].mass = 0.05f;//m_points[0].vel = vec3;//m_points[0].pos += dt*m_throwSpeed;m_points[m_pointNum-1].mass = 0;if (m_length>1){m_length -= m_pullSpeed*dt;float springLength = m_length/(m_pointNum-1);for (int i = 0; i < m_pointNum - 1; ++i)          {m_springs[i].springLen = springLength;}}//m_points[m_pointNum-1].vel = vec3();//m_points[m_pointNum-1].pos += dt*m_throwSpeed;}Solve(dt);}
}void PhyObjRope::Solve(float dt)
{//计算各质点力for (int i = 0; i < m_pointNum; ++i)     m_points[i].ClearForce();   //弹簧力for (int i = 0; i < m_pointNum - 1; ++i)         {m_springs[i].Solve();                  }龙的骨骼各质点不能超过头部//for (int i = 1; i < m_pointNum; ++i)          //{//   vec3 dif = m_points[0].pos - m_points[i].pos;  //  //dif.d//}//其它力for (int i = 0; i < m_pointNum; ++i)               {if (m_points[i].mass==StaticMass) //静态质点{continue;}//重力m_points[i].ApplyForce(gravity * m_points[i].mass);   // 空气摩擦力m_points[i].ApplyForce(-m_points[i].vel * airFriction);if (m_points[i].pos.y < groundHeight)     {vec3 v = m_points[i].vel;             v.y = 0;                   //地面摩擦力m_points[i].ApplyForce(-v * groundFriction);v = m_points[i].vel;                v.x = 0;                   v.z = 0;                   if (v.y < 0)                 {//地面缓冲力m_points[i].ApplyForce(-v * groundAbsorption);}//地面反作用力vec3 force = vec3(0, groundRepulsion, 0) * (groundHeight - m_points[i].pos.y);m_points[i].ApplyForce(force);            }}for (int i = 0; i < m_pointNum; ++i)        {if (m_points[i].mass==0) //静态质点{continue;}m_points[i].Simulate(dt);}//for (int i=0;i<m_boneNum;i++)//{//   mat.Identity();//   mat.FromToDir(FaceAxis,m_points[i].vel);//  matB.FromTranslate(m_points[i].pos);//  mat = matB*mat;//  m_bodyParts[i].rigidBody->Set(mat);//}}PhyObjRope::Point* PhyObjRope::GetPoint(int index)
{if (index < 0 || index >= m_pointNum)       return NULL;                            return &m_points[index];
}void PhyObjRope::SetControlType(ControlType controlType)
{m_controlType = controlType;
}void PhyObjRope::BeginThrow(const vec3& pos,const vec3& speed)
{m_throwPos = pos;m_throwSpeed = speed;m_length = (m_throwPos-m_points[m_pointNum-1].pos).Length()/1.1f;float springLength = m_length/(m_pointNum-1);for (int i = 0; i < m_pointNum - 1; ++i)         {m_springs[i].springLen = springLength;}SetControlType(Throw);
}void PhyObjRope::BeginSwing(float len,float speed)
{m_swingRadius = len;m_pullSpeed = speed;SetControlType(Swing);
}void PhyObjRope::BeginPull(const vec3& pos,float speed)
{m_pullSpeed = speed;m_points[m_pointNum-1].pos = pos;SetControlType(Pull);
}}

水体

#include "General/Pch.h"
#include "General/General.h"
#include "General/Smoother.h"
#include "Physic/PhyMesh.h"
#include "Physic/PhyObject.h"
#include "Physic/PhyObjWater.h"
#include "Physic/RigidBody.h"
#include "Physic/FindContacts.h"
#include "Physic/PhysicWorld.h"
#include "Render/RendDriver.h"
#include "Render/MC_MovieClip.h"
#include "Render/MC_Misc.h"
#include "Render/Water.h"
#include "General/Pce.h"
#include "Math/MathLibAdvance.h"static inline float TetrahedronVolume(vec3& c, const vec3& p, const vec3& v1, const vec3& v2, const vec3& v3)
{//vec3 a = v2 - v1;//vec3 b = v3 - v1;//vec3 r = p - v1;//float volume = (1.0f/6.0f)*Cross(b,a).Dot(r);float a[3];float b[3];float r[3];MinusFloat3(a, ((float*)&v2.x),((float*)&v1.x));MinusFloat3(b, ((float*)&v3.x),((float*)&v1.x));CrossFloat3(r, b,a);MinusFloat3(a, ((float*)&p.x),((float*)&v1.x) );//四面体体积float volume = (1.0f/6.0f)*DotFloat3(r,a);//质心？float volDev4 = volume*0.25f;c.x += volDev4*(v1.x + v2.x + v3.x + p.x);c.y += volDev4*(v1.y + v2.y + v3.y + p.y);c.z += volDev4*(v1.z + v2.z + v3.z + p.z);return volume;
}static inline float ClipTriangle(  vec3& cen, vec3 p,vec3 v1, vec3 v2, vec3 v3,float d1, float d2, float d3)
{// Clips a partially submerged triangle and returns the volume of the// resulting tetrahedrons and updates the centroid accumulator.assert(d1*d2 < 0);vec3 vc1 = v1 + (d1/(d1 - d2))*(v2 - v1);float volume = 0;if (d1 < 0){if (d3 < 0){// Case B - a quadrilateral or two triangles.vec3 vc2 = v2 + (d2/(d2 - d3))*(v3 - v2);volume += TetrahedronVolume(cen, p, vc1, vc2, v1);volume += TetrahedronVolume(cen, p, vc2, v3, v1);}else{// Case A - a single triangle.vec3 vc2 = v1 + (d1/(d1 - d3))*(v3 - v1);volume += TetrahedronVolume(cen, p, vc1, vc2, v1);}}else{if (d3 < 0){// Case Bvec3 vc2 = v1 + (d1/(d1 - d3))*(v3 - v1);volume += TetrahedronVolume(cen, p, vc1, v2, v3);volume += TetrahedronVolume(cen, p, vc1, v3, vc2);}else{// Case Avec3 vc2 = v2 + (d2/(d2 - d3))*(v3 - v2);volume += TetrahedronVolume(cen, p, vc1, v2, vc2);}}return volume;
}namespace Physic
{PhysicObjectWater::PhysicObjectWater(const char* name):PhyObject(name,ObjectWater)
{}PhysicObjectWater::~PhysicObjectWater()
{SafeDelete(m_waterPlane);
}
void  PhysicObjectWater::Init(const char* filename)
{m_density     = 1.0f; //水的密度m_angularDrag = 0.5f;m_linearDrag  = 5.0f;m_velocity    = vec3();m_waterPlane = new WaterGerstner;m_waterPlane->LoadFromFile(filename);
}//void PhysicObjectWater::OnAdded()
//{
//  if (m_rigidBody)
//  {
//      m_phyWorld->AddRigidBody(m_rigidBody);
//  }
//}
//void PhysicObjectWater::OnRemove()
//{
//}void PhysicObjectWater::SetPos(const vec3 &pos)
{m_pos = pos;m_waterPlane->SetPos(pos,vec3(0,400,400)/*G_Camera->GetEyePos()*/,0,true);
}void  PhysicObjectWater::Render()
{if (m_waterPlane){m_waterPlane->Render();}else{G_RendDriver->EnableRendState(RS_BLEND);G_RendDriver->Color4f(0.0f, 0.85f, 0.95f, 1.0f);float y = m_pos.y + 0.01f;G_RendDriver->RendBegin(RS_QUADS);G_RendDriver->Normal3f(0, 1, 0);G_RendDriver->Vertex3f(-100, y, -100);G_RendDriver->Vertex3f(-100, y, 100);G_RendDriver->Vertex3f(100, y, 100);G_RendDriver->Vertex3f(100, y, -100);G_RendDriver->RendEnd();}
}void  PhysicObjectWater::Update(float ifps)
{if (m_waterPlane){m_waterPlane->Update(ifps);}
}int  PhysicObjectWater::Intersection(const vec3 &line0, const vec3 &line1, vec3 &point, vec3 &normal)
{//PhyMesh* mesh = m_rigidBody?(m_rigidBody->m_collide->m_phyMeshW):m_phyMesh;//vec3 p, n;//if(mesh->Intersection(line0, line1, p, n))//{//   point = p;//   normal = n;//  return true;//}return false;
}
void PhysicObjectWater::EditRender()
{}const vec3 & PhysicObjectWater::getWMin(int s /*= -1*/)
{return m_pos;
}const vec3 & PhysicObjectWater::getWMax(int s /*= -1*/)
{return m_pos;
}const vec3 & PhysicObjectWater::GetWCenter(int s /*= -1*/)
{return m_pos;
}float PhysicObjectWater::getRadius(int s)
{return 0;//m_phyMesh->getRadius(s);
}float PhysicObjectWater::SubmergedVolume(const Plane2& waterPlane,RigidBody& body,vec3& buoyancyCenter)
{//计算下沉体积 浮心将水面waterPlane从世界坐标系变换到刚体空间  使用的是meshW 这一步可以省略//quat qt = body.m_rotQ;//qt.Conjugate(); //inverse//vec3  normal = qt.Rotate(waterPlane.n);//float offset = waterPlane.d - waterPlane.n.Dot(body.m_massCenter);vec3  normal = waterPlane.n;float offset = waterPlane.d;float TINY_DEPTH = -1e-6f;//点相对于水面的高度int numSubmerged = 0;int sampleVert = 0;PhySurface* surface = body.m_collide->m_phyMeshW->m_surfaces[0];PhyVertex*  vertex  = surface->vertex;for (int i = 0; i < surface->vertexNum; ++i,vertex++){vertex->ds = normal.Dot(vertex->pos) - offset;if (vertex->ds < TINY_DEPTH){++numSubmerged;sampleVert = i;}}if (numSubmerged == 0){    //所有点都在水面上buoyancyCenter = vec3(0,0,0);return 0;}else if (numSubmerged == surface->vertexNum){    //所有点都在水面下  密度均匀物体 浮心和重心重合//buoyancyCenter = vec3(0,0,0);//return 0;}//在水面上找一个点的投影点 提高精确度。//此点用作计算所有的四面体体积。所有的水面三角形都是零体积四面体。vec3 prj = surface->vertex[sampleVert].pos - surface->vertex[sampleVert].ds*normal;//float volume = 0;buoyancyCenter = vec3(0,0,0);//计算总淹没体积PhyTriangle* tri = surface->triangles;for (int i = 0; i < surface->trigonNum; ++i,tri++){vec3& v1 = tri->vertexPtr[0]->pos;float d1 = tri->vertexPtr[0]->ds;vec3& v2 = tri->vertexPtr[1]->pos;float d2 = tri->vertexPtr[1]->ds;vec3& v3 = tri->vertexPtr[2]->pos;float d3 = tri->vertexPtr[2]->ds;if (d1 * d2 < 0){// v1-v2 crosses the watter planevolume += ClipTriangle(buoyancyCenter, prj, v1, v2, v3, d1, d2, d3);}else if (d1 * d3 < 0){// v1-v3 crosses the planevolume += ClipTriangle(buoyancyCenter, prj, v3, v1, v2, d3, d1, d2);}else if (d2 * d3 < 0){// v2-v3 crosses the planevolume += ClipTriangle(buoyancyCenter, prj, v2, v3, v1, d2, d3, d1);}else if (d1 < 0 || d2 < 0 || d3 < 0){// 完全淹没fully submergedvolume += TetrahedronVolume(buoyancyCenter, prj, v1, v2, v3);}}//0或负体积未沾水  float TINY_VOLUME = 1e-6f;if (volume <= TINY_VOLUME){buoyancyCenter = vec3(0,0,0);return 0;}//NormalizebuoyancyCenter *= 1.0f/volume;将浮心从刚体空间变换到世界坐标系  使用的是meshW 这一步可以省略//buoyancyCenter = body.m_massCenter + body.m_rotQ.Rotate(buoyancyCenter);buoyancyCenter = buoyancyCenter;return volume;
}void  PhysicObjectWater::CalcBuoyancy(RigidBody& body)
{vec2 posCen(body.m_massCenter.x,body.m_massCenter.z);vec2 posCen_(posCen);posCen_.x -= m_waterPlane->GetPos().x;posCen_.y -= m_waterPlane->GetPos().z;if (abs(posCen_.x) > m_waterPlane->GetExtend().x*0.49f||abs(posCen_.y) > m_waterPlane->GetExtend().y*0.49f){return;}Plane2 waterPlane;waterPlane.d = 0;waterPlane.n = vec3(0,1,0);if (m_waterPlane){//刚体所处位置的水面waterPlane.n = m_waterPlane->GetNormalLinear(posCen);float height = m_waterPlane->GetHeightLinear(posCen);waterPlane.d = vec3(body.m_massCenter.x,height,body.m_massCenter.z).Dot(waterPlane.n);}//浮心vec3  buoyancyCenter;//排水量float mergedVolume = SubmergedVolume(waterPlane,body, buoyancyCenter);if (mergedVolume > 0){float gravity = m_phyWorld->Gravity.Length();//98 9.8f;//浮力 大小=排水的重力 方向沿着法线（将水面简化为微平面）vec3 buoyancyForce = (m_density*mergedVolume*gravity) * waterPlane.n;//水对物体的线阻力//float partialMass = body.m_mass * mergedVolume / body.getVolume();float partialMass = m_density * mergedVolume;vec3 rc = buoyancyCenter - body.m_massCenter;//力矩vec3 vc = body.m_lineVelocity + Cross(body.m_angVelocity , rc);//点速度vec3 dragForce = (partialMass* m_linearDrag)*( m_velocity - vc);//m_linearDrag = 1.8f;//vec3 dragForce = (partialMass* m_linearDrag)*( m_velocity - vc)*( m_velocity - vc).Length();vec3 totalForce = buoyancyForce + dragForce;body.m_forces  += totalForce;body.m_torques += Cross(rc , totalForce);//旋转阻力float lenSq = body.getRadius();lenSq *= lenSq;vec3 dragTorque = (-partialMass* m_angularDrag*lenSq)*body.m_angVelocity;body.m_torques += dragTorque;}
}}

收尾

游戏引擎设计 - 物理（Crapell Game Engine Design - physic）相关推荐

3D游戏引擎设计实时计算机图形学的应用方法第2版 pdf 带索引书签目录
3D游戏引擎设计实时计算机图形学的应用方法第2版目录第1章概述 1.1 图形硬件和游戏发展史 1.2 本书版本与软件发展史 1.3 章节导读第2章图形系统 2.1 基础知识 2.1. ...
游戏引擎设计 - 粒子系统
粒子系统是一个游戏引擎必不可少的部分,粒子系统的实现并没有统一的标准,各类3D动画软件和游戏引擎的实现也不尽相同. 本套粒子系统基于3ds max的标准,实现了max9绝大部分的粒子参数调节功能.暂不 ...
ACT游戏引擎设计及DEMO展示
声明:这个DEMO中所用的图片来自己GL的游戏,只是为了学习研究之用,请下载测试过后把游戏删除.谢谢! 说明:这段时间工作不是很忙,利用业余时间把前两年在手机游戏公司的一些经验和积累做一个总结,写了这 ...
游戏动画引擎 -（Crapell Game Engine Design - animation）
动画重定向和逆向运动学: Biovision Hierarchy, 是BioVision公司推出的人体动作捕捉文件格式,天然具有重定向功能.首先实现BVHSkeleton类来加载bvh文件(包含骨架和 ...
Ophone平台2D游戏引擎实现——物理引擎（一）（二）
http://dev.10086.cn/cmdn/wiki/index.php?doc-view-4271.html http://dev.10086.cn/cmdn/wiki/index.php?d ...
3D游戏引擎设计--实时计算机图形学的应用方法
2015-03-06 这本书是非常好的参考资料,我在做最近的一个任务,需要在CAD程序中把全局坐标映射到局部坐标,本来对这个计算过程还是有一点模糊的,按照书中相关章节的分析,没花多久就实现出来了.虽然 ...
ARPG游戏引擎设计思路
http://bbs.9ria.com/thread-108554-1-1.html 运用alchemy技术提高寻路算法近10倍速度这个真霸道啊,留个标记,改天学习下. http://bbs.9ri ...
支持树莓派Raspberry Pi的Delphi开源3D游戏引擎CGE：Castle Game Engine
最近我得到了我的Raspberry Pi 4设备,我对这些可能性感到兴奋(具有全功能Linux的小型便宜设备).我很高兴看到Castle Game Engine在它上面完美运行?以下是在Raspber ...
模型驱动架构探索之游戏引擎设计（二）粒度统一
粒度统一与建模: 为屏蔽人为编译时代码构建复杂,业务难以统一的现象,实现运行时自动化,将编码转移为"配置数据"形式,系统自动通过装配器将模型与配置进行原型装配,实体管理器获取原型模 ...
（十一）Flax Engine游戏引擎物理引擎 joints （关节）
2021SC@SDUSC 本次我们继续针对flax Engine 游戏引擎中物理引擎的joints 包进行源代码的分析.上次我们分析了距离铰链和固定铰链,本次我们将针对flax Engine 中最后一 ...

游戏引擎设计 - 物理（Crapell Game Engine Design - physic）

动能

游戏引擎设计 - 物理（Crapell Game Engine Design - physic）相关推荐

最新文章

热门文章