介绍了一款小型游戏物理引擎，特色包括浮力效果、动态龙形动画及布娃娃系统的物理与动画融合。提及未实现连续碰撞检测的不足，并讨论了理论基础如力的合成与分解、力矩等。优化策略涉及包围盒剔除、BSPTree及缓存计算结果，提升稳定性措施。C++实现细节展示，如刚体定义及遇到的问题。
这是一个小型的游戏物理引擎，大部分功能都有比如刚体、连接、布娃娃系统。
稍微有些特色的地方：
添加了浮力的效果，
布娃娃系统可以响应预制动画和物理双重模拟，
添加了一个自适应龙形动画。
从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 TestNotValidNum
namespace Physic
{
#define StaticMass 0
class 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的radius
float m_radius;
//初始重置
vec3 m_resetPos;
vec3 m_resetRot;
PhyMesh* m_phyMesh;
#define m_phyWorld m_phyWorld
PhysicWorld* 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_phyMesh
SafeDelete(m_phyMesh);
SafeDelete(m_collide);
}
void RigidBody::SetPos(const vec3 &pos)
{
//update bsp
m_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 bsp
m_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 bsp
if (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 contact
for(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);
}
// friction
if(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; // 质点1
Point* point2; // 质点2
float 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();//dsc
file.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();
//读取skin
file.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 name
file.ReadString(m_bodyParts[i].boneName,64);//bone name
m_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");
}
{
//辅助轴movie
for (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 joint
m_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 body
frame.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 frame
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 = 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 body
frame.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再matTransform
mat4 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); //更新纯动画matrix
m_movieSkin->Advance(time); //更新纯动画matrix
float 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对应的刚体matrix
mat4 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_joints
m_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);
//相移BoneLocal
for (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;//80
vec3 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的某浮点值curvePos
accumPosLen = 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 B
vec3 vc2 = v1 + (d1/(d1 - d3))*(v3 - v1);
volume += TetrahedronVolume(cen, p, vc1, v2, v3);
volume += TetrahedronVolume(cen, p, vc1, v3, vc2);
}
else
{
// Case A
vec3 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 plane
volume += ClipTriangle(buoyancyCenter, prj, v1, v2, v3, d1, d2, d3);
}
else if (d1 * d3 < 0)
{
// v1-v3 crosses the plane
volume += ClipTriangle(buoyancyCenter, prj, v3, v1, v2, d3, d1, d2);
}
else if (d2 * d3 < 0)
{
// v2-v3 crosses the plane
volume += ClipTriangle(buoyancyCenter, prj, v2, v3, v1, d2, d3, d1);
}
else if (d1 < 0 || d2 < 0 || d3 < 0)
{
// 完全淹没fully submerged
volume += TetrahedronVolume(buoyancyCenter, prj, v1, v2, v3);
}
}
//0或负体积未沾水
float TINY_VOLUME = 1e-6f;
if (volume <= TINY_VOLUME)
{
buoyancyCenter = vec3(0,0,0);
return 0;
}
//Normalize
buoyancyCenter *= 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;
}
}
}
收尾
扫一扫
1248
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
