2005-07-18 05:41:00 +00:00
|
|
|
#include "CcdPhysicsEnvironment.h"
|
|
|
|
#include "CcdPhysicsController.h"
|
|
|
|
|
|
|
|
#include <algorithm>
|
|
|
|
#include "SimdTransform.h"
|
|
|
|
#include "Dynamics/RigidBody.h"
|
2005-08-02 18:54:11 +00:00
|
|
|
#include "BroadphaseCollision/BroadphaseInterface.h"
|
2005-07-18 05:41:00 +00:00
|
|
|
#include "BroadphaseCollision/SimpleBroadphase.h"
|
2006-03-27 06:37:30 +00:00
|
|
|
#include "BroadphaseCollision/AxisSweep3.h"
|
|
|
|
|
2006-02-21 05:36:56 +00:00
|
|
|
#include "CollisionDispatch/CollisionWorld.h"
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
#include "CollisionShapes/ConvexShape.h"
|
2006-02-21 05:36:56 +00:00
|
|
|
#include "BroadphaseCollision/Dispatcher.h"
|
2005-07-18 05:41:00 +00:00
|
|
|
#include "NarrowPhaseCollision/PersistentManifold.h"
|
|
|
|
#include "CollisionShapes/TriangleMeshShape.h"
|
|
|
|
#include "ConstraintSolver/OdeConstraintSolver.h"
|
|
|
|
#include "ConstraintSolver/SimpleConstraintSolver.h"
|
|
|
|
|
2006-04-13 05:11:34 +00:00
|
|
|
#ifdef USE_PROFILE
|
|
|
|
//profiling/timings
|
|
|
|
#include "quickprof.h"
|
|
|
|
#endif //USE_PROFILE
|
2006-02-21 05:36:56 +00:00
|
|
|
|
2005-07-29 18:14:41 +00:00
|
|
|
#include "IDebugDraw.h"
|
2005-07-18 05:41:00 +00:00
|
|
|
|
2005-08-05 17:00:32 +00:00
|
|
|
#include "NarrowPhaseCollision/VoronoiSimplexSolver.h"
|
2005-08-18 06:04:50 +00:00
|
|
|
#include "NarrowPhaseCollision/SubSimplexConvexCast.h"
|
2006-02-21 05:36:56 +00:00
|
|
|
#include "NarrowPhaseCollision/GjkConvexCast.h"
|
2006-04-23 18:42:17 +00:00
|
|
|
#include "NarrowPhaseCollision/ContinuousConvexCollision.h"
|
2006-02-21 05:36:56 +00:00
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
|
2006-02-21 05:36:56 +00:00
|
|
|
#include "CollisionDispatch/CollisionDispatcher.h"
|
2006-02-13 06:28:35 +00:00
|
|
|
#include "PHY_IMotionState.h"
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
#include "CollisionDispatch/EmptyCollisionAlgorithm.h"
|
|
|
|
#include "CollisionDispatch/UnionFind.h"
|
|
|
|
|
|
|
|
#include "NarrowPhaseCollision/RaycastCallback.h"
|
2005-08-05 17:00:32 +00:00
|
|
|
#include "CollisionShapes/SphereShape.h"
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
bool useIslands = true;
|
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
#ifdef NEW_BULLET_VEHICLE_SUPPORT
|
|
|
|
#include "Vehicle/RaycastVehicle.h"
|
|
|
|
#include "Vehicle/VehicleRaycaster.h"
|
2006-02-21 05:36:56 +00:00
|
|
|
|
|
|
|
#include "Vehicle/WheelInfo.h"
|
2006-02-13 06:28:35 +00:00
|
|
|
#include "PHY_IVehicle.h"
|
2006-02-21 05:36:56 +00:00
|
|
|
RaycastVehicle::VehicleTuning gTuning;
|
2006-02-13 06:28:35 +00:00
|
|
|
|
|
|
|
#endif //NEW_BULLET_VEHICLE_SUPPORT
|
|
|
|
#include "AabbUtil2.h"
|
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
#include "ConstraintSolver/ConstraintSolver.h"
|
|
|
|
#include "ConstraintSolver/Point2PointConstraint.h"
|
2006-04-06 20:37:38 +00:00
|
|
|
#include "ConstraintSolver/HingeConstraint.h"
|
|
|
|
|
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
//#include "BroadphaseCollision/QueryDispatcher.h"
|
|
|
|
//#include "BroadphaseCollision/QueryBox.h"
|
|
|
|
//todo: change this to allow dynamic registration of types!
|
|
|
|
|
|
|
|
#ifdef WIN32
|
|
|
|
void DrawRasterizerLine(const float* from,const float* to,int color);
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
#include "ConstraintSolver/ContactConstraint.h"
|
|
|
|
|
|
|
|
|
|
|
|
#include <stdio.h>
|
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
#ifdef NEW_BULLET_VEHICLE_SUPPORT
|
|
|
|
class WrapperVehicle : public PHY_IVehicle
|
|
|
|
{
|
|
|
|
|
|
|
|
RaycastVehicle* m_vehicle;
|
|
|
|
PHY_IPhysicsController* m_chassis;
|
|
|
|
|
|
|
|
public:
|
|
|
|
|
|
|
|
WrapperVehicle(RaycastVehicle* vehicle,PHY_IPhysicsController* chassis)
|
|
|
|
:m_vehicle(vehicle),
|
|
|
|
m_chassis(chassis)
|
|
|
|
{
|
|
|
|
}
|
|
|
|
|
|
|
|
RaycastVehicle* GetVehicle()
|
|
|
|
{
|
|
|
|
return m_vehicle;
|
|
|
|
}
|
|
|
|
|
|
|
|
PHY_IPhysicsController* GetChassis()
|
|
|
|
{
|
|
|
|
return m_chassis;
|
|
|
|
}
|
|
|
|
|
|
|
|
virtual void AddWheel(
|
|
|
|
PHY_IMotionState* motionState,
|
|
|
|
PHY__Vector3 connectionPoint,
|
|
|
|
PHY__Vector3 downDirection,
|
|
|
|
PHY__Vector3 axleDirection,
|
|
|
|
float suspensionRestLength,
|
|
|
|
float wheelRadius,
|
|
|
|
bool hasSteering
|
|
|
|
)
|
|
|
|
{
|
|
|
|
SimdVector3 connectionPointCS0(connectionPoint[0],connectionPoint[1],connectionPoint[2]);
|
|
|
|
SimdVector3 wheelDirectionCS0(downDirection[0],downDirection[1],downDirection[2]);
|
|
|
|
SimdVector3 wheelAxle(axleDirection[0],axleDirection[1],axleDirection[2]);
|
|
|
|
|
|
|
|
|
|
|
|
WheelInfo& info = m_vehicle->AddWheel(connectionPointCS0,wheelDirectionCS0,wheelAxle,
|
|
|
|
suspensionRestLength,wheelRadius,gTuning,hasSteering);
|
|
|
|
info.m_clientInfo = motionState;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
void SyncWheels()
|
|
|
|
{
|
|
|
|
int numWheels = GetNumWheels();
|
|
|
|
int i;
|
|
|
|
for (i=0;i<numWheels;i++)
|
|
|
|
{
|
|
|
|
WheelInfo& info = m_vehicle->GetWheelInfo(i);
|
|
|
|
PHY_IMotionState* motionState = (PHY_IMotionState*)info.m_clientInfo ;
|
2006-02-22 06:58:05 +00:00
|
|
|
m_vehicle->UpdateWheelTransform(i);
|
2006-02-13 06:28:35 +00:00
|
|
|
SimdTransform trans = m_vehicle->GetWheelTransformWS(i);
|
|
|
|
SimdQuaternion orn = trans.getRotation();
|
|
|
|
const SimdVector3& pos = trans.getOrigin();
|
|
|
|
motionState->setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]);
|
|
|
|
motionState->setWorldPosition(pos.x(),pos.y(),pos.z());
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
virtual int GetNumWheels() const
|
|
|
|
{
|
|
|
|
return m_vehicle->GetNumWheels();
|
|
|
|
}
|
|
|
|
|
|
|
|
virtual void GetWheelPosition(int wheelIndex,float& posX,float& posY,float& posZ) const
|
|
|
|
{
|
|
|
|
SimdTransform trans = m_vehicle->GetWheelTransformWS(wheelIndex);
|
|
|
|
posX = trans.getOrigin().x();
|
|
|
|
posY = trans.getOrigin().y();
|
|
|
|
posZ = trans.getOrigin().z();
|
|
|
|
}
|
|
|
|
virtual void GetWheelOrientationQuaternion(int wheelIndex,float& quatX,float& quatY,float& quatZ,float& quatW) const
|
|
|
|
{
|
|
|
|
SimdTransform trans = m_vehicle->GetWheelTransformWS(wheelIndex);
|
|
|
|
SimdQuaternion quat = trans.getRotation();
|
|
|
|
SimdMatrix3x3 orn2(quat);
|
|
|
|
|
|
|
|
quatX = trans.getRotation().x();
|
|
|
|
quatY = trans.getRotation().y();
|
|
|
|
quatZ = trans.getRotation().z();
|
|
|
|
quatW = trans.getRotation()[3];
|
|
|
|
|
|
|
|
|
|
|
|
//printf("test");
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
virtual float GetWheelRotation(int wheelIndex) const
|
|
|
|
{
|
|
|
|
float rotation = 0.f;
|
|
|
|
|
|
|
|
if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
|
|
|
|
{
|
|
|
|
WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
|
|
|
|
rotation = info.m_rotation;
|
|
|
|
}
|
|
|
|
return rotation;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
virtual int GetUserConstraintId() const
|
|
|
|
{
|
|
|
|
return m_vehicle->GetUserConstraintId();
|
|
|
|
}
|
|
|
|
|
|
|
|
virtual int GetUserConstraintType() const
|
|
|
|
{
|
|
|
|
return m_vehicle->GetUserConstraintType();
|
|
|
|
}
|
|
|
|
|
|
|
|
virtual void SetSteeringValue(float steering,int wheelIndex)
|
|
|
|
{
|
|
|
|
m_vehicle->SetSteeringValue(steering,wheelIndex);
|
|
|
|
}
|
|
|
|
|
|
|
|
virtual void ApplyEngineForce(float force,int wheelIndex)
|
|
|
|
{
|
|
|
|
m_vehicle->ApplyEngineForce(force,wheelIndex);
|
|
|
|
}
|
|
|
|
|
|
|
|
virtual void ApplyBraking(float braking,int wheelIndex)
|
|
|
|
{
|
|
|
|
if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
|
|
|
|
{
|
|
|
|
WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
|
|
|
|
info.m_brake = braking;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2006-02-21 07:08:23 +00:00
|
|
|
virtual void SetWheelFriction(float friction,int wheelIndex)
|
|
|
|
{
|
|
|
|
if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
|
|
|
|
{
|
|
|
|
WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
|
|
|
|
info.m_frictionSlip = friction;
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
virtual void SetSuspensionStiffness(float suspensionStiffness,int wheelIndex)
|
|
|
|
{
|
|
|
|
if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
|
|
|
|
{
|
|
|
|
WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
|
|
|
|
info.m_suspensionStiffness = suspensionStiffness;
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
virtual void SetSuspensionDamping(float suspensionDamping,int wheelIndex)
|
|
|
|
{
|
|
|
|
if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
|
|
|
|
{
|
|
|
|
WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
|
|
|
|
info.m_wheelsDampingRelaxation = suspensionDamping;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
virtual void SetSuspensionCompression(float suspensionCompression,int wheelIndex)
|
|
|
|
{
|
|
|
|
if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
|
|
|
|
{
|
|
|
|
WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
|
|
|
|
info.m_wheelsDampingCompression = suspensionCompression;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
virtual void SetRollInfluence(float rollInfluence,int wheelIndex)
|
|
|
|
{
|
|
|
|
if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
|
|
|
|
{
|
|
|
|
WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
|
|
|
|
info.m_rollInfluence = rollInfluence;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
};
|
|
|
|
#endif //NEW_BULLET_VEHICLE_SUPPORT
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
|
2005-07-27 09:30:53 +00:00
|
|
|
|
2005-07-29 18:14:41 +00:00
|
|
|
static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdVector3& to,const SimdVector3& color)
|
2005-07-27 09:30:53 +00:00
|
|
|
{
|
|
|
|
SimdVector3 halfExtents = (to-from)* 0.5f;
|
|
|
|
SimdVector3 center = (to+from) *0.5f;
|
|
|
|
int i,j;
|
|
|
|
|
|
|
|
SimdVector3 edgecoord(1.f,1.f,1.f),pa,pb;
|
|
|
|
for (i=0;i<4;i++)
|
|
|
|
{
|
|
|
|
for (j=0;j<3;j++)
|
|
|
|
{
|
|
|
|
pa = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],
|
|
|
|
edgecoord[2]*halfExtents[2]);
|
|
|
|
pa+=center;
|
|
|
|
|
|
|
|
int othercoord = j%3;
|
|
|
|
edgecoord[othercoord]*=-1.f;
|
|
|
|
pb = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],
|
|
|
|
edgecoord[2]*halfExtents[2]);
|
|
|
|
pb+=center;
|
|
|
|
|
|
|
|
debugDrawer->DrawLine(pa,pb,color);
|
|
|
|
}
|
|
|
|
edgecoord = SimdVector3(-1.f,-1.f,-1.f);
|
|
|
|
if (i<3)
|
|
|
|
edgecoord[i]*=-1.f;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
2006-02-21 05:36:56 +00:00
|
|
|
CcdPhysicsEnvironment::CcdPhysicsEnvironment(CollisionDispatcher* dispatcher,BroadphaseInterface* broadphase)
|
|
|
|
:m_scalingPropagated(false),
|
2006-03-29 03:11:30 +00:00
|
|
|
m_numIterations(10),
|
2005-08-04 19:07:39 +00:00
|
|
|
m_ccdMode(0),
|
2006-04-13 05:11:34 +00:00
|
|
|
m_solverType(-1),
|
|
|
|
m_profileTimings(0),
|
|
|
|
m_enableSatCollisionDetection(false)
|
2005-07-18 05:41:00 +00:00
|
|
|
{
|
2005-08-04 19:07:39 +00:00
|
|
|
|
2006-04-17 01:33:10 +00:00
|
|
|
for (int i=0;i<PHY_NUM_RESPONSE;i++)
|
|
|
|
{
|
|
|
|
m_triggerCallbacks[i] = 0;
|
|
|
|
}
|
2006-02-21 05:36:56 +00:00
|
|
|
if (!dispatcher)
|
|
|
|
dispatcher = new CollisionDispatcher();
|
2005-08-04 19:07:39 +00:00
|
|
|
|
2006-02-21 05:36:56 +00:00
|
|
|
|
|
|
|
if(!broadphase)
|
2006-03-27 06:37:30 +00:00
|
|
|
{
|
|
|
|
|
|
|
|
//todo: calculate/let user specify this world sizes
|
|
|
|
SimdVector3 worldMin(-10000,-10000,-10000);
|
|
|
|
SimdVector3 worldMax(10000,10000,10000);
|
|
|
|
|
|
|
|
broadphase = new AxisSweep3(worldMin,worldMax);
|
|
|
|
|
|
|
|
//broadphase = new SimpleBroadphase();
|
|
|
|
}
|
2006-02-21 05:36:56 +00:00
|
|
|
|
2006-04-06 20:37:38 +00:00
|
|
|
|
2006-04-01 03:30:15 +00:00
|
|
|
setSolverType(1);
|
2006-02-21 05:36:56 +00:00
|
|
|
|
|
|
|
m_collisionWorld = new CollisionWorld(dispatcher,broadphase);
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
m_debugDrawer = 0;
|
|
|
|
m_gravity = SimdVector3(0.f,-10.f,0.f);
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
void CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
|
|
|
|
{
|
2006-02-21 05:36:56 +00:00
|
|
|
RigidBody* body = ctrl->GetRigidBody();
|
2006-04-17 01:33:10 +00:00
|
|
|
|
|
|
|
//this m_userPointer is just used for triggers, see CallbackTriggers
|
|
|
|
body->m_userPointer = ctrl;
|
2006-02-21 05:36:56 +00:00
|
|
|
|
|
|
|
body->setGravity( m_gravity );
|
2005-07-18 05:41:00 +00:00
|
|
|
m_controllers.push_back(ctrl);
|
|
|
|
|
2006-02-21 05:36:56 +00:00
|
|
|
m_collisionWorld->AddCollisionObject(body);
|
|
|
|
|
|
|
|
assert(body->m_broadphaseHandle);
|
|
|
|
|
|
|
|
BroadphaseInterface* scene = GetBroadphase();
|
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
CollisionShape* shapeinterface = ctrl->GetCollisionShape();
|
|
|
|
|
|
|
|
assert(shapeinterface);
|
|
|
|
|
|
|
|
const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
|
|
|
|
|
|
|
|
|
|
|
|
SimdPoint3 minAabb,maxAabb;
|
|
|
|
|
|
|
|
shapeinterface->GetAabb(t,minAabb,maxAabb);
|
|
|
|
|
|
|
|
float timeStep = 0.02f;
|
|
|
|
|
|
|
|
|
|
|
|
//extent it with the motion
|
|
|
|
|
|
|
|
SimdVector3 linMotion = body->getLinearVelocity()*timeStep;
|
|
|
|
|
|
|
|
float maxAabbx = maxAabb.getX();
|
|
|
|
float maxAabby = maxAabb.getY();
|
|
|
|
float maxAabbz = maxAabb.getZ();
|
|
|
|
float minAabbx = minAabb.getX();
|
|
|
|
float minAabby = minAabb.getY();
|
|
|
|
float minAabbz = minAabb.getZ();
|
|
|
|
|
|
|
|
if (linMotion.x() > 0.f)
|
|
|
|
maxAabbx += linMotion.x();
|
|
|
|
else
|
|
|
|
minAabbx += linMotion.x();
|
|
|
|
if (linMotion.y() > 0.f)
|
|
|
|
maxAabby += linMotion.y();
|
|
|
|
else
|
|
|
|
minAabby += linMotion.y();
|
|
|
|
if (linMotion.z() > 0.f)
|
|
|
|
maxAabbz += linMotion.z();
|
|
|
|
else
|
|
|
|
minAabbz += linMotion.z();
|
|
|
|
|
|
|
|
|
|
|
|
minAabb = SimdVector3(minAabbx,minAabby,minAabbz);
|
|
|
|
maxAabb = SimdVector3(maxAabbx,maxAabby,maxAabbz);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
void CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl)
|
|
|
|
{
|
|
|
|
|
|
|
|
//also remove constraint
|
|
|
|
|
|
|
|
{
|
2006-04-06 20:37:38 +00:00
|
|
|
std::vector<TypedConstraint*>::iterator i;
|
2005-07-18 05:41:00 +00:00
|
|
|
|
2006-04-06 20:37:38 +00:00
|
|
|
for (i=m_constraints.begin();
|
|
|
|
!(i==m_constraints.end()); i++)
|
2005-07-18 05:41:00 +00:00
|
|
|
{
|
2006-04-06 20:37:38 +00:00
|
|
|
TypedConstraint* constraint = (*i);
|
|
|
|
if ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() ||
|
|
|
|
(&constraint->GetRigidBodyB() == ctrl->GetRigidBody())))
|
2005-07-18 05:41:00 +00:00
|
|
|
{
|
2006-04-06 20:37:38 +00:00
|
|
|
removeConstraint(constraint->GetUserConstraintId());
|
2005-07-18 05:41:00 +00:00
|
|
|
//only 1 constraint per constroller
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
{
|
2006-04-06 20:37:38 +00:00
|
|
|
std::vector<TypedConstraint*>::iterator i;
|
2005-07-18 05:41:00 +00:00
|
|
|
|
2006-04-06 20:37:38 +00:00
|
|
|
for (i=m_constraints.begin();
|
|
|
|
!(i==m_constraints.end()); i++)
|
2005-07-18 05:41:00 +00:00
|
|
|
{
|
2006-04-06 20:37:38 +00:00
|
|
|
TypedConstraint* constraint = (*i);
|
|
|
|
if ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() ||
|
|
|
|
(&constraint->GetRigidBodyB() == ctrl->GetRigidBody())))
|
2005-07-18 05:41:00 +00:00
|
|
|
{
|
2006-04-06 20:37:38 +00:00
|
|
|
removeConstraint(constraint->GetUserConstraintId());
|
2005-07-18 05:41:00 +00:00
|
|
|
//only 1 constraint per constroller
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2006-02-21 05:36:56 +00:00
|
|
|
m_collisionWorld->RemoveCollisionObject(ctrl->GetRigidBody());
|
2005-12-31 07:20:08 +00:00
|
|
|
|
2006-02-21 05:36:56 +00:00
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
{
|
|
|
|
std::vector<CcdPhysicsController*>::iterator i =
|
|
|
|
std::find(m_controllers.begin(), m_controllers.end(), ctrl);
|
|
|
|
if (!(i == m_controllers.end()))
|
|
|
|
{
|
|
|
|
std::swap(*i, m_controllers.back());
|
|
|
|
m_controllers.pop_back();
|
|
|
|
}
|
|
|
|
}
|
2006-04-17 01:33:10 +00:00
|
|
|
|
|
|
|
//remove it from the triggers
|
|
|
|
{
|
|
|
|
std::vector<CcdPhysicsController*>::iterator i =
|
|
|
|
std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl);
|
|
|
|
if (!(i == m_triggerControllers.end()))
|
|
|
|
{
|
|
|
|
std::swap(*i, m_triggerControllers.back());
|
|
|
|
m_triggerControllers.pop_back();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2006-01-01 00:20:50 +00:00
|
|
|
void CcdPhysicsEnvironment::beginFrame()
|
|
|
|
{
|
2005-07-18 05:41:00 +00:00
|
|
|
|
2006-01-01 00:20:50 +00:00
|
|
|
}
|
2005-07-18 05:41:00 +00:00
|
|
|
|
2006-04-13 05:11:34 +00:00
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
|
|
|
|
{
|
2006-01-15 11:34:55 +00:00
|
|
|
|
2006-04-13 05:11:34 +00:00
|
|
|
#ifdef USE_PROFILE
|
|
|
|
//toggle Profiler
|
|
|
|
if ( m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_ProfileTimings)
|
|
|
|
{
|
|
|
|
if (!m_profileTimings)
|
|
|
|
{
|
|
|
|
m_profileTimings = 1;
|
|
|
|
// To disable profiling, simply comment out the following line.
|
|
|
|
static int counter = 0;
|
|
|
|
|
|
|
|
char filename[128];
|
|
|
|
sprintf(filename,"quickprof_bullet_timings%i.csv",counter++);
|
|
|
|
Profiler::init(filename, Profiler::BLOCK_CYCLE_SECONDS);//BLOCK_TOTAL_MICROSECONDS
|
|
|
|
|
|
|
|
}
|
|
|
|
} else
|
|
|
|
{
|
|
|
|
if (m_profileTimings)
|
|
|
|
{
|
|
|
|
m_profileTimings = 0;
|
|
|
|
Profiler::destroy();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif //USE_PROFILE
|
|
|
|
|
|
|
|
|
|
|
|
|
2006-01-15 11:34:55 +00:00
|
|
|
if (!SimdFuzzyZero(timeStep))
|
|
|
|
{
|
2006-04-06 20:37:38 +00:00
|
|
|
|
2006-04-13 05:11:34 +00:00
|
|
|
// define this in blender, the stepsize is 30 hertz, 60 hertz works much better
|
|
|
|
#define SPLIT_TIMESTEP 1
|
|
|
|
|
2006-04-06 20:37:38 +00:00
|
|
|
#ifdef SPLIT_TIMESTEP
|
2006-01-15 11:34:55 +00:00
|
|
|
proceedDeltaTimeOneStep(0.5f*timeStep);
|
|
|
|
proceedDeltaTimeOneStep(0.5f*timeStep);
|
2006-04-06 20:37:38 +00:00
|
|
|
#else
|
|
|
|
proceedDeltaTimeOneStep(timeStep);
|
|
|
|
#endif
|
2006-01-15 11:34:55 +00:00
|
|
|
} else
|
|
|
|
{
|
|
|
|
//todo: interpolate
|
|
|
|
}
|
2006-04-13 05:11:34 +00:00
|
|
|
|
2006-01-15 11:34:55 +00:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
/// Perform an integration step of duration 'timeStep'.
|
|
|
|
bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
|
|
|
|
{
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
|
2005-08-13 08:05:48 +00:00
|
|
|
// printf("CcdPhysicsEnvironment::proceedDeltaTime\n");
|
2005-07-18 05:41:00 +00:00
|
|
|
|
2006-01-12 06:30:01 +00:00
|
|
|
if (SimdFuzzyZero(timeStep))
|
2005-07-18 05:41:00 +00:00
|
|
|
return true;
|
|
|
|
|
2005-08-13 08:05:48 +00:00
|
|
|
if (m_debugDrawer)
|
|
|
|
{
|
|
|
|
gDisableDeactivation = (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_NoDeactivation);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2006-04-13 05:11:34 +00:00
|
|
|
#ifdef USE_PROFILE
|
|
|
|
Profiler::beginBlock("SyncMotionStates");
|
|
|
|
#endif //USE_PROFILE
|
2005-08-08 17:08:42 +00:00
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
//this is needed because scaling is not known in advance, and scaling has to propagate to the shape
|
|
|
|
if (!m_scalingPropagated)
|
|
|
|
{
|
2006-01-01 00:20:50 +00:00
|
|
|
SyncMotionStates(timeStep);
|
|
|
|
m_scalingPropagated = true;
|
2005-07-18 05:41:00 +00:00
|
|
|
}
|
|
|
|
|
2006-04-13 05:11:34 +00:00
|
|
|
#ifdef USE_PROFILE
|
|
|
|
Profiler::endBlock("SyncMotionStates");
|
2005-07-18 05:41:00 +00:00
|
|
|
|
2006-04-13 05:11:34 +00:00
|
|
|
Profiler::beginBlock("predictIntegratedTransform");
|
|
|
|
#endif //USE_PROFILE
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
{
|
|
|
|
// std::vector<CcdPhysicsController*>::iterator i;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int k;
|
|
|
|
for (k=0;k<GetNumControllers();k++)
|
|
|
|
{
|
|
|
|
CcdPhysicsController* ctrl = m_controllers[k];
|
|
|
|
// SimdTransform predictedTrans;
|
|
|
|
RigidBody* body = ctrl->GetRigidBody();
|
|
|
|
if (body->GetActivationState() != ISLAND_SLEEPING)
|
|
|
|
{
|
|
|
|
body->applyForces( timeStep);
|
|
|
|
body->integrateVelocities( timeStep);
|
2006-02-21 05:36:56 +00:00
|
|
|
body->predictIntegratedTransform(timeStep,body->m_nextPredictedWorldTransform);
|
2005-07-18 05:41:00 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
2006-04-13 05:11:34 +00:00
|
|
|
|
|
|
|
#ifdef USE_PROFILE
|
|
|
|
Profiler::endBlock("predictIntegratedTransform");
|
|
|
|
#endif //USE_PROFILE
|
|
|
|
|
2006-02-21 05:36:56 +00:00
|
|
|
BroadphaseInterface* scene = GetBroadphase();
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
|
|
|
|
//
|
|
|
|
// collision detection (?)
|
|
|
|
//
|
|
|
|
|
|
|
|
|
2006-04-13 05:11:34 +00:00
|
|
|
#ifdef USE_PROFILE
|
|
|
|
Profiler::beginBlock("DispatchAllCollisionPairs");
|
|
|
|
#endif //USE_PROFILE
|
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
|
2005-08-03 18:22:30 +00:00
|
|
|
int numsubstep = m_numIterations;
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
|
|
|
|
DispatcherInfo dispatchInfo;
|
|
|
|
dispatchInfo.m_timeStep = timeStep;
|
|
|
|
dispatchInfo.m_stepCount = 0;
|
2006-04-13 05:11:34 +00:00
|
|
|
dispatchInfo.m_enableSatConvex = m_enableSatCollisionDetection;
|
2005-07-18 05:41:00 +00:00
|
|
|
|
2006-02-21 05:36:56 +00:00
|
|
|
scene->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);///numsubstep,g);
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
|
2006-04-13 05:11:34 +00:00
|
|
|
#ifdef USE_PROFILE
|
|
|
|
Profiler::endBlock("DispatchAllCollisionPairs");
|
|
|
|
#endif //USE_PROFILE
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
|
|
|
|
int numRigidBodies = m_controllers.size();
|
|
|
|
|
2006-02-21 05:36:56 +00:00
|
|
|
m_collisionWorld->UpdateActivationState();
|
|
|
|
|
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
//contacts
|
2006-04-13 05:11:34 +00:00
|
|
|
#ifdef USE_PROFILE
|
|
|
|
Profiler::beginBlock("SolveConstraint");
|
|
|
|
#endif //USE_PROFILE
|
2005-07-29 18:14:41 +00:00
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
|
2006-04-06 20:37:38 +00:00
|
|
|
//solve the regular constraints (point 2 point, hinge, etc)
|
|
|
|
|
|
|
|
for (int g=0;g<numsubstep;g++)
|
|
|
|
{
|
|
|
|
//
|
|
|
|
// constraint solving
|
|
|
|
//
|
2006-04-13 05:11:34 +00:00
|
|
|
|
2006-04-06 20:37:38 +00:00
|
|
|
|
|
|
|
int i;
|
|
|
|
int numConstraints = m_constraints.size();
|
|
|
|
|
|
|
|
//point to point constraints
|
|
|
|
for (i=0;i< numConstraints ; i++ )
|
|
|
|
{
|
|
|
|
TypedConstraint* constraint = m_constraints[i];
|
|
|
|
|
|
|
|
constraint->BuildJacobian();
|
|
|
|
constraint->SolveConstraint( timeStep );
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
}
|
2006-04-13 05:11:34 +00:00
|
|
|
|
|
|
|
#ifdef USE_PROFILE
|
|
|
|
Profiler::endBlock("SolveConstraint");
|
|
|
|
#endif //USE_PROFILE
|
2006-04-06 20:37:38 +00:00
|
|
|
|
|
|
|
//solve the vehicles
|
|
|
|
|
|
|
|
#ifdef NEW_BULLET_VEHICLE_SUPPORT
|
|
|
|
//vehicles
|
|
|
|
int numVehicles = m_wrapperVehicles.size();
|
|
|
|
for (int i=0;i<numVehicles;i++)
|
|
|
|
{
|
|
|
|
WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
|
|
|
|
RaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
|
|
|
|
vehicle->UpdateVehicle( timeStep);
|
|
|
|
}
|
|
|
|
#endif //NEW_BULLET_VEHICLE_SUPPORT
|
|
|
|
|
|
|
|
|
|
|
|
struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback
|
2006-02-21 05:36:56 +00:00
|
|
|
{
|
|
|
|
|
|
|
|
ContactSolverInfo& m_solverInfo;
|
|
|
|
ConstraintSolver* m_solver;
|
|
|
|
IDebugDraw* m_debugDrawer;
|
|
|
|
|
|
|
|
InplaceSolverIslandCallback(
|
|
|
|
ContactSolverInfo& solverInfo,
|
|
|
|
ConstraintSolver* solver,
|
|
|
|
IDebugDraw* debugDrawer)
|
|
|
|
:m_solverInfo(solverInfo),
|
|
|
|
m_solver(solver),
|
|
|
|
m_debugDrawer(debugDrawer)
|
|
|
|
{
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
virtual void ProcessIsland(PersistentManifold** manifolds,int numManifolds)
|
|
|
|
{
|
|
|
|
m_solver->SolveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer);
|
|
|
|
}
|
|
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
m_solverInfo.m_friction = 0.9f;
|
|
|
|
m_solverInfo.m_numIterations = m_numIterations;
|
|
|
|
m_solverInfo.m_timeStep = timeStep;
|
|
|
|
m_solverInfo.m_restitution = 0.f;//m_restitution;
|
|
|
|
|
|
|
|
InplaceSolverIslandCallback solverCallback(
|
|
|
|
m_solverInfo,
|
|
|
|
m_solver,
|
|
|
|
m_debugDrawer);
|
|
|
|
|
2006-04-13 05:11:34 +00:00
|
|
|
#ifdef USE_PROFILE
|
|
|
|
Profiler::beginBlock("BuildAndProcessIslands");
|
|
|
|
#endif //USE_PROFILE
|
|
|
|
|
2006-04-06 20:37:38 +00:00
|
|
|
/// solve all the contact points and contact friction
|
2006-02-21 05:36:56 +00:00
|
|
|
GetDispatcher()->BuildAndProcessIslands(numRigidBodies,&solverCallback);
|
|
|
|
|
2006-04-13 05:11:34 +00:00
|
|
|
#ifdef USE_PROFILE
|
|
|
|
Profiler::endBlock("BuildAndProcessIslands");
|
2006-04-17 01:33:10 +00:00
|
|
|
|
|
|
|
Profiler::beginBlock("CallbackTriggers");
|
|
|
|
#endif //USE_PROFILE
|
|
|
|
|
|
|
|
CallbackTriggers();
|
|
|
|
|
|
|
|
#ifdef USE_PROFILE
|
|
|
|
Profiler::endBlock("CallbackTriggers");
|
|
|
|
|
|
|
|
|
2006-04-13 05:11:34 +00:00
|
|
|
Profiler::beginBlock("proceedToTransform");
|
2005-07-18 05:41:00 +00:00
|
|
|
|
2006-04-13 05:11:34 +00:00
|
|
|
#endif //USE_PROFILE
|
2005-07-18 05:41:00 +00:00
|
|
|
{
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
std::vector<CcdPhysicsController*>::iterator i;
|
|
|
|
|
|
|
|
//
|
|
|
|
// update aabbs, only for moving objects (!)
|
|
|
|
//
|
|
|
|
for (i=m_controllers.begin();
|
|
|
|
!(i==m_controllers.end()); i++)
|
|
|
|
{
|
|
|
|
CcdPhysicsController* ctrl = (*i);
|
|
|
|
RigidBody* body = ctrl->GetRigidBody();
|
|
|
|
|
|
|
|
|
|
|
|
SimdPoint3 minAabb,maxAabb;
|
|
|
|
CollisionShape* shapeinterface = ctrl->GetCollisionShape();
|
2005-07-27 09:30:53 +00:00
|
|
|
|
|
|
|
|
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(),
|
|
|
|
body->getLinearVelocity(),body->getAngularVelocity(),
|
|
|
|
timeStep,minAabb,maxAabb);
|
|
|
|
|
2005-07-27 09:30:53 +00:00
|
|
|
shapeinterface->GetAabb(body->getCenterOfMassTransform(),
|
|
|
|
minAabb,maxAabb);
|
|
|
|
|
2005-08-13 08:05:48 +00:00
|
|
|
SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
|
|
|
|
minAabb -= manifoldExtraExtents;
|
|
|
|
maxAabb += manifoldExtraExtents;
|
2005-07-18 05:41:00 +00:00
|
|
|
|
2006-02-21 05:36:56 +00:00
|
|
|
BroadphaseProxy* bp = body->m_broadphaseHandle;
|
2005-07-18 05:41:00 +00:00
|
|
|
if (bp)
|
|
|
|
{
|
|
|
|
|
|
|
|
#ifdef WIN32
|
2005-08-13 08:05:48 +00:00
|
|
|
SimdVector3 color (1,1,0);
|
2005-08-04 19:07:39 +00:00
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
if (m_debugDrawer)
|
2005-07-27 09:30:53 +00:00
|
|
|
{
|
|
|
|
//draw aabb
|
2005-08-04 19:07:39 +00:00
|
|
|
switch (body->GetActivationState())
|
|
|
|
{
|
|
|
|
case ISLAND_SLEEPING:
|
|
|
|
{
|
2005-08-13 08:05:48 +00:00
|
|
|
color.setValue(1,1,1);
|
2005-08-04 19:07:39 +00:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
case WANTS_DEACTIVATION:
|
|
|
|
{
|
|
|
|
color.setValue(0,0,1);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case ACTIVE_TAG:
|
|
|
|
{
|
|
|
|
break;
|
|
|
|
}
|
2006-02-22 06:58:05 +00:00
|
|
|
case DISABLE_DEACTIVATION:
|
|
|
|
{
|
|
|
|
color.setValue(1,0,1);
|
|
|
|
};
|
2005-08-04 19:07:39 +00:00
|
|
|
|
|
|
|
};
|
2005-07-27 09:30:53 +00:00
|
|
|
|
2005-08-13 08:05:48 +00:00
|
|
|
if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
|
|
|
|
{
|
|
|
|
DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
|
|
|
|
}
|
2005-07-27 09:30:53 +00:00
|
|
|
}
|
2005-07-18 05:41:00 +00:00
|
|
|
#endif
|
2005-08-04 19:07:39 +00:00
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
scene->SetAabb(bp,minAabb,maxAabb);
|
2005-08-04 19:07:39 +00:00
|
|
|
|
|
|
|
|
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
float toi = 1.f;
|
|
|
|
|
|
|
|
|
|
|
|
|
2005-08-03 18:22:30 +00:00
|
|
|
if (m_ccdMode == 3)
|
2005-07-18 05:41:00 +00:00
|
|
|
{
|
|
|
|
DispatcherInfo dispatchInfo;
|
|
|
|
dispatchInfo.m_timeStep = timeStep;
|
|
|
|
dispatchInfo.m_stepCount = 0;
|
|
|
|
dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
|
|
|
|
|
2006-02-21 05:36:56 +00:00
|
|
|
scene->DispatchAllCollisionPairs( *GetDispatcher(),dispatchInfo);///numsubstep,g);
|
2005-07-18 05:41:00 +00:00
|
|
|
toi = dispatchInfo.m_timeOfImpact;
|
|
|
|
}
|
|
|
|
|
|
|
|
//
|
|
|
|
// integrating solution
|
|
|
|
//
|
|
|
|
|
|
|
|
{
|
|
|
|
std::vector<CcdPhysicsController*>::iterator i;
|
|
|
|
|
|
|
|
for (i=m_controllers.begin();
|
|
|
|
!(i==m_controllers.end()); i++)
|
|
|
|
{
|
|
|
|
|
|
|
|
CcdPhysicsController* ctrl = *i;
|
|
|
|
|
|
|
|
SimdTransform predictedTrans;
|
|
|
|
RigidBody* body = ctrl->GetRigidBody();
|
|
|
|
if (body->GetActivationState() != ISLAND_SLEEPING)
|
|
|
|
{
|
2006-02-21 05:36:56 +00:00
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
body->predictIntegratedTransform(timeStep* toi, predictedTrans);
|
|
|
|
body->proceedToTransform( predictedTrans);
|
2006-02-21 05:36:56 +00:00
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//
|
|
|
|
// disable sleeping physics objects
|
|
|
|
//
|
|
|
|
|
|
|
|
std::vector<CcdPhysicsController*> m_sleepingControllers;
|
|
|
|
|
|
|
|
for (i=m_controllers.begin();
|
|
|
|
!(i==m_controllers.end()); i++)
|
|
|
|
{
|
|
|
|
CcdPhysicsController* ctrl = (*i);
|
|
|
|
RigidBody* body = ctrl->GetRigidBody();
|
2005-08-04 19:07:39 +00:00
|
|
|
|
|
|
|
ctrl->UpdateDeactivation(timeStep);
|
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
if (ctrl->wantsSleeping())
|
|
|
|
{
|
|
|
|
if (body->GetActivationState() == ACTIVE_TAG)
|
|
|
|
body->SetActivationState( WANTS_DEACTIVATION );
|
|
|
|
} else
|
|
|
|
{
|
2006-02-22 06:58:05 +00:00
|
|
|
if (body->GetActivationState() != DISABLE_DEACTIVATION)
|
|
|
|
body->SetActivationState( ACTIVE_TAG );
|
2005-07-18 05:41:00 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
if (useIslands)
|
|
|
|
{
|
|
|
|
if (body->GetActivationState() == ISLAND_SLEEPING)
|
|
|
|
{
|
|
|
|
m_sleepingControllers.push_back(ctrl);
|
|
|
|
}
|
|
|
|
} else
|
|
|
|
{
|
|
|
|
if (ctrl->wantsSleeping())
|
|
|
|
{
|
|
|
|
m_sleepingControllers.push_back(ctrl);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
|
2006-04-13 05:11:34 +00:00
|
|
|
#ifdef USE_PROFILE
|
|
|
|
Profiler::endBlock("proceedToTransform");
|
|
|
|
|
|
|
|
Profiler::beginBlock("SyncMotionStates");
|
|
|
|
#endif //USE_PROFILE
|
2006-02-13 06:28:35 +00:00
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
SyncMotionStates(timeStep);
|
|
|
|
|
2006-04-13 05:11:34 +00:00
|
|
|
#ifdef USE_PROFILE
|
|
|
|
Profiler::endBlock("SyncMotionStates");
|
|
|
|
|
|
|
|
Profiler::endProfilingCycle();
|
|
|
|
#endif //USE_PROFILE
|
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
|
|
|
|
#ifdef NEW_BULLET_VEHICLE_SUPPORT
|
|
|
|
//sync wheels for vehicles
|
|
|
|
int numVehicles = m_wrapperVehicles.size();
|
|
|
|
for (int i=0;i<numVehicles;i++)
|
|
|
|
{
|
|
|
|
WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
|
2006-02-22 06:58:05 +00:00
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
wrapperVehicle->SyncWheels();
|
|
|
|
}
|
|
|
|
#endif //NEW_BULLET_VEHICLE_SUPPORT
|
2005-07-18 05:41:00 +00:00
|
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2005-07-29 18:14:41 +00:00
|
|
|
void CcdPhysicsEnvironment::setDebugMode(int debugMode)
|
|
|
|
{
|
|
|
|
if (m_debugDrawer){
|
|
|
|
m_debugDrawer->SetDebugMode(debugMode);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2005-08-03 18:22:30 +00:00
|
|
|
void CcdPhysicsEnvironment::setNumIterations(int numIter)
|
|
|
|
{
|
|
|
|
m_numIterations = numIter;
|
|
|
|
}
|
|
|
|
void CcdPhysicsEnvironment::setDeactivationTime(float dTime)
|
|
|
|
{
|
|
|
|
gDeactivationTime = dTime;
|
|
|
|
}
|
|
|
|
void CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh)
|
|
|
|
{
|
|
|
|
gLinearSleepingTreshold = linTresh;
|
|
|
|
}
|
|
|
|
void CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh)
|
|
|
|
{
|
|
|
|
gAngularSleepingTreshold = angTresh;
|
|
|
|
}
|
|
|
|
void CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
|
|
|
|
{
|
|
|
|
gContactBreakingTreshold = contactBreakingTreshold;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void CcdPhysicsEnvironment::setCcdMode(int ccdMode)
|
|
|
|
{
|
|
|
|
m_ccdMode = ccdMode;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void CcdPhysicsEnvironment::setSolverSorConstant(float sor)
|
|
|
|
{
|
2006-02-21 05:36:56 +00:00
|
|
|
m_solverInfo.m_sor = sor;
|
2005-08-03 18:22:30 +00:00
|
|
|
}
|
|
|
|
|
2005-08-04 19:07:39 +00:00
|
|
|
void CcdPhysicsEnvironment::setSolverTau(float tau)
|
2005-08-03 18:22:30 +00:00
|
|
|
{
|
2006-02-21 05:36:56 +00:00
|
|
|
m_solverInfo.m_tau = tau;
|
2005-08-03 18:22:30 +00:00
|
|
|
}
|
2005-08-04 19:07:39 +00:00
|
|
|
void CcdPhysicsEnvironment::setSolverDamping(float damping)
|
2005-08-03 18:22:30 +00:00
|
|
|
{
|
2006-02-21 05:36:56 +00:00
|
|
|
m_solverInfo.m_damping = damping;
|
2005-08-03 18:22:30 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2005-08-04 19:07:39 +00:00
|
|
|
void CcdPhysicsEnvironment::setLinearAirDamping(float damping)
|
|
|
|
{
|
|
|
|
gLinearAirDamping = damping;
|
|
|
|
}
|
|
|
|
|
|
|
|
void CcdPhysicsEnvironment::setUseEpa(bool epa)
|
|
|
|
{
|
|
|
|
gUseEpa = epa;
|
|
|
|
}
|
|
|
|
|
|
|
|
void CcdPhysicsEnvironment::setSolverType(int solverType)
|
|
|
|
{
|
|
|
|
|
|
|
|
switch (solverType)
|
|
|
|
{
|
|
|
|
case 1:
|
|
|
|
{
|
|
|
|
if (m_solverType != solverType)
|
|
|
|
{
|
2006-02-21 05:36:56 +00:00
|
|
|
|
|
|
|
m_solver = new SimpleConstraintSolver();
|
2006-04-06 20:37:38 +00:00
|
|
|
|
2005-08-04 19:07:39 +00:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
case 0:
|
|
|
|
default:
|
|
|
|
if (m_solverType != solverType)
|
|
|
|
{
|
2006-02-21 05:36:56 +00:00
|
|
|
m_solver = new OdeConstraintSolver();
|
|
|
|
|
2005-08-04 19:07:39 +00:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
m_solverType = solverType ;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
2005-08-03 18:22:30 +00:00
|
|
|
|
2005-07-29 18:14:41 +00:00
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
void CcdPhysicsEnvironment::SyncMotionStates(float timeStep)
|
|
|
|
{
|
|
|
|
std::vector<CcdPhysicsController*>::iterator i;
|
|
|
|
|
|
|
|
//
|
|
|
|
// synchronize the physics and graphics transformations
|
|
|
|
//
|
2005-08-05 17:00:32 +00:00
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
for (i=m_controllers.begin();
|
|
|
|
!(i==m_controllers.end()); i++)
|
|
|
|
{
|
|
|
|
CcdPhysicsController* ctrl = (*i);
|
|
|
|
ctrl->SynchronizeMotionStates(timeStep);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
void CcdPhysicsEnvironment::setGravity(float x,float y,float z)
|
|
|
|
{
|
|
|
|
m_gravity = SimdVector3(x,y,z);
|
|
|
|
|
|
|
|
std::vector<CcdPhysicsController*>::iterator i;
|
|
|
|
|
|
|
|
//todo: review this gravity stuff
|
|
|
|
for (i=m_controllers.begin();
|
|
|
|
!(i==m_controllers.end()); i++)
|
|
|
|
{
|
|
|
|
|
|
|
|
CcdPhysicsController* ctrl = (*i);
|
|
|
|
ctrl->GetRigidBody()->setGravity(m_gravity);
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
#ifdef NEW_BULLET_VEHICLE_SUPPORT
|
|
|
|
|
2006-04-06 20:37:38 +00:00
|
|
|
class DefaultVehicleRaycaster : public VehicleRaycaster
|
2006-02-13 06:28:35 +00:00
|
|
|
{
|
|
|
|
CcdPhysicsEnvironment* m_physEnv;
|
|
|
|
PHY_IPhysicsController* m_chassis;
|
|
|
|
|
|
|
|
public:
|
2006-04-06 20:37:38 +00:00
|
|
|
DefaultVehicleRaycaster(CcdPhysicsEnvironment* physEnv,PHY_IPhysicsController* chassis):
|
2006-02-13 06:28:35 +00:00
|
|
|
m_physEnv(physEnv),
|
|
|
|
m_chassis(chassis)
|
|
|
|
{
|
|
|
|
}
|
|
|
|
|
|
|
|
/* struct VehicleRaycasterResult
|
|
|
|
{
|
|
|
|
VehicleRaycasterResult() :m_distFraction(-1.f){};
|
|
|
|
SimdVector3 m_hitPointInWorld;
|
|
|
|
SimdVector3 m_hitNormalInWorld;
|
|
|
|
SimdScalar m_distFraction;
|
|
|
|
};
|
|
|
|
*/
|
|
|
|
virtual void* CastRay(const SimdVector3& from,const SimdVector3& to, VehicleRaycasterResult& result)
|
|
|
|
{
|
|
|
|
|
|
|
|
|
|
|
|
float hit[3];
|
|
|
|
float normal[3];
|
|
|
|
|
|
|
|
PHY_IPhysicsController* ignore = m_chassis;
|
|
|
|
void* hitObject = m_physEnv->rayTest(ignore,from.x(),from.y(),from.z(),to.x(),to.y(),to.z(),hit[0],hit[1],hit[2],normal[0],normal[1],normal[2]);
|
|
|
|
if (hitObject)
|
|
|
|
{
|
|
|
|
result.m_hitPointInWorld[0] = hit[0];
|
|
|
|
result.m_hitPointInWorld[1] = hit[1];
|
|
|
|
result.m_hitPointInWorld[2] = hit[2];
|
|
|
|
result.m_hitNormalInWorld[0] = normal[0];
|
|
|
|
result.m_hitNormalInWorld[1] = normal[1];
|
|
|
|
result.m_hitNormalInWorld[2] = normal[2];
|
|
|
|
result.m_hitNormalInWorld.normalize();
|
|
|
|
//calc fraction? or put it in the interface?
|
|
|
|
//calc for now
|
|
|
|
|
|
|
|
result.m_distFraction = (result.m_hitPointInWorld-from).length() / (to-from).length();
|
|
|
|
//some safety for 'explosion' due to sudden penetration of the full 'ray'
|
|
|
|
/* if (result.m_distFraction<0.1)
|
|
|
|
{
|
|
|
|
printf("Vehicle Raycast: avoided instability due to penetration. Consider moving the connection points deeper inside vehicle chassis");
|
|
|
|
result.m_distFraction = 1.f;
|
|
|
|
hitObject = 0;
|
|
|
|
}
|
|
|
|
*/
|
|
|
|
|
|
|
|
/* if (result.m_distFraction>1.)
|
|
|
|
{
|
|
|
|
printf("Vehicle Raycast: avoided instability 1Consider moving the connection points deeper inside vehicle chassis");
|
|
|
|
result.m_distFraction = 1.f;
|
|
|
|
hitObject = 0;
|
|
|
|
}
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
//?
|
|
|
|
return hitObject;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif //NEW_BULLET_VEHICLE_SUPPORT
|
|
|
|
|
|
|
|
static int gConstraintUid = 1;
|
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
|
|
|
|
float pivotX,float pivotY,float pivotZ,
|
|
|
|
float axisX,float axisY,float axisZ)
|
|
|
|
{
|
|
|
|
|
|
|
|
|
|
|
|
CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
|
|
|
|
CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
|
|
|
|
|
|
|
|
RigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
|
|
|
|
RigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
|
|
|
|
|
|
|
|
ASSERT(rb0);
|
|
|
|
|
|
|
|
SimdVector3 pivotInA(pivotX,pivotY,pivotZ);
|
|
|
|
SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
|
2006-04-06 20:37:38 +00:00
|
|
|
SimdVector3 axisInA(axisX,axisY,axisZ);
|
|
|
|
SimdVector3 axisInB = rb1 ?
|
|
|
|
(rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) :
|
|
|
|
rb0->getCenterOfMassTransform().getBasis() * -axisInA;
|
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
switch (type)
|
|
|
|
{
|
|
|
|
case PHY_POINT2POINT_CONSTRAINT:
|
|
|
|
{
|
|
|
|
|
|
|
|
Point2PointConstraint* p2p = 0;
|
|
|
|
|
|
|
|
if (rb1)
|
|
|
|
{
|
|
|
|
p2p = new Point2PointConstraint(*rb0,
|
|
|
|
*rb1,pivotInA,pivotInB);
|
|
|
|
} else
|
|
|
|
{
|
|
|
|
p2p = new Point2PointConstraint(*rb0,
|
|
|
|
pivotInA);
|
|
|
|
}
|
|
|
|
|
2006-04-06 20:37:38 +00:00
|
|
|
m_constraints.push_back(p2p);
|
2006-02-13 06:28:35 +00:00
|
|
|
p2p->SetUserConstraintId(gConstraintUid++);
|
|
|
|
p2p->SetUserConstraintType(type);
|
2006-01-15 11:34:55 +00:00
|
|
|
//64 bit systems can't cast pointer to int. could use size_t instead.
|
2006-02-13 06:28:35 +00:00
|
|
|
return p2p->GetUserConstraintId();
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
break;
|
|
|
|
}
|
2006-04-06 20:37:38 +00:00
|
|
|
|
|
|
|
case PHY_LINEHINGE_CONSTRAINT:
|
|
|
|
{
|
|
|
|
HingeConstraint* hinge = 0;
|
|
|
|
|
|
|
|
if (rb1)
|
|
|
|
{
|
|
|
|
hinge = new HingeConstraint(
|
|
|
|
*rb0,
|
|
|
|
*rb1,pivotInA,pivotInB,axisInA,axisInB);
|
|
|
|
|
|
|
|
|
|
|
|
} else
|
|
|
|
{
|
|
|
|
hinge = new HingeConstraint(*rb0,
|
|
|
|
pivotInA,axisInA);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
m_constraints.push_back(hinge);
|
|
|
|
hinge->SetUserConstraintId(gConstraintUid++);
|
|
|
|
hinge->SetUserConstraintType(type);
|
|
|
|
//64 bit systems can't cast pointer to int. could use size_t instead.
|
|
|
|
return hinge->GetUserConstraintId();
|
|
|
|
break;
|
|
|
|
}
|
2006-02-13 06:28:35 +00:00
|
|
|
#ifdef NEW_BULLET_VEHICLE_SUPPORT
|
|
|
|
|
|
|
|
case PHY_VEHICLE_CONSTRAINT:
|
|
|
|
{
|
2006-02-21 05:36:56 +00:00
|
|
|
RaycastVehicle::VehicleTuning* tuning = new RaycastVehicle::VehicleTuning();
|
2006-02-13 06:28:35 +00:00
|
|
|
RigidBody* chassis = rb0;
|
2006-04-06 20:37:38 +00:00
|
|
|
DefaultVehicleRaycaster* raycaster = new DefaultVehicleRaycaster(this,ctrl0);
|
2006-02-13 06:28:35 +00:00
|
|
|
RaycastVehicle* vehicle = new RaycastVehicle(*tuning,chassis,raycaster);
|
|
|
|
WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
|
|
|
|
m_wrapperVehicles.push_back(wrapperVehicle);
|
|
|
|
vehicle->SetUserConstraintId(gConstraintUid++);
|
|
|
|
vehicle->SetUserConstraintType(type);
|
|
|
|
return vehicle->GetUserConstraintId();
|
|
|
|
|
|
|
|
break;
|
|
|
|
};
|
|
|
|
#endif //NEW_BULLET_VEHICLE_SUPPORT
|
|
|
|
|
2005-07-18 05:41:00 +00:00
|
|
|
default:
|
|
|
|
{
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
//RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
2006-01-15 11:34:55 +00:00
|
|
|
void CcdPhysicsEnvironment::removeConstraint(int constraintId)
|
2005-07-18 05:41:00 +00:00
|
|
|
{
|
2006-04-06 20:37:38 +00:00
|
|
|
std::vector<TypedConstraint*>::iterator i;
|
2005-07-18 05:41:00 +00:00
|
|
|
|
2006-04-06 20:37:38 +00:00
|
|
|
for (i=m_constraints.begin();
|
|
|
|
!(i==m_constraints.end()); i++)
|
2006-01-15 11:34:55 +00:00
|
|
|
{
|
2006-04-06 20:37:38 +00:00
|
|
|
TypedConstraint* constraint = (*i);
|
|
|
|
if (constraint->GetUserConstraintId() == constraintId)
|
2006-01-15 11:34:55 +00:00
|
|
|
{
|
2006-04-06 20:37:38 +00:00
|
|
|
std::swap(*i, m_constraints.back());
|
|
|
|
m_constraints.pop_back();
|
2006-01-15 11:34:55 +00:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
}
|
|
|
|
PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ,
|
|
|
|
float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
|
|
|
|
{
|
|
|
|
|
2005-08-17 19:52:56 +00:00
|
|
|
float minFraction = 1.f;
|
2005-07-18 05:41:00 +00:00
|
|
|
|
2005-08-05 17:00:32 +00:00
|
|
|
SimdTransform rayFromTrans,rayToTrans;
|
|
|
|
rayFromTrans.setIdentity();
|
2006-02-13 06:28:35 +00:00
|
|
|
|
|
|
|
SimdVector3 rayFrom(fromX,fromY,fromZ);
|
|
|
|
|
|
|
|
rayFromTrans.setOrigin(rayFrom);
|
2005-08-05 17:00:32 +00:00
|
|
|
rayToTrans.setIdentity();
|
2006-02-13 06:28:35 +00:00
|
|
|
SimdVector3 rayTo(toX,toY,toZ);
|
|
|
|
rayToTrans.setOrigin(rayTo);
|
2005-08-05 17:00:32 +00:00
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
//do culling based on aabb (rayFrom/rayTo)
|
|
|
|
SimdVector3 rayAabbMin = rayFrom;
|
|
|
|
SimdVector3 rayAabbMax = rayFrom;
|
|
|
|
rayAabbMin.setMin(rayTo);
|
|
|
|
rayAabbMax.setMax(rayTo);
|
2005-08-05 17:00:32 +00:00
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
|
2005-08-05 17:00:32 +00:00
|
|
|
CcdPhysicsController* nearestHit = 0;
|
|
|
|
|
|
|
|
std::vector<CcdPhysicsController*>::iterator i;
|
|
|
|
SphereShape pointShape(0.0f);
|
|
|
|
|
|
|
|
/// brute force go over all objects. Once there is a broadphase, use that, or
|
|
|
|
/// add a raycast against aabb first.
|
|
|
|
for (i=m_controllers.begin();
|
|
|
|
!(i==m_controllers.end()); i++)
|
|
|
|
{
|
|
|
|
CcdPhysicsController* ctrl = (*i);
|
2006-02-13 06:28:35 +00:00
|
|
|
if (ctrl == ignoreClient)
|
|
|
|
continue;
|
2005-08-05 17:00:32 +00:00
|
|
|
RigidBody* body = ctrl->GetRigidBody();
|
2006-02-13 06:28:35 +00:00
|
|
|
SimdVector3 bodyAabbMin,bodyAabbMax;
|
|
|
|
body->getAabb(bodyAabbMin,bodyAabbMax);
|
2005-08-05 17:00:32 +00:00
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
//check aabb overlap
|
2005-08-05 17:00:32 +00:00
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,bodyAabbMin,bodyAabbMax))
|
|
|
|
{
|
|
|
|
if (body->GetCollisionShape()->IsConvex())
|
2005-08-05 17:00:32 +00:00
|
|
|
{
|
2006-02-13 06:28:35 +00:00
|
|
|
ConvexCast::CastResult rayResult;
|
|
|
|
rayResult.m_fraction = 1.f;
|
|
|
|
|
|
|
|
ConvexShape* convexShape = (ConvexShape*) body->GetCollisionShape();
|
|
|
|
VoronoiSimplexSolver simplexSolver;
|
2006-04-23 18:42:17 +00:00
|
|
|
//SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
|
2006-02-21 05:36:56 +00:00
|
|
|
//GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
|
2006-04-23 18:42:17 +00:00
|
|
|
ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
|
2006-02-13 06:28:35 +00:00
|
|
|
|
|
|
|
if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,body->getCenterOfMassTransform(),body->getCenterOfMassTransform(),rayResult))
|
2005-08-05 17:00:32 +00:00
|
|
|
{
|
2006-02-13 06:28:35 +00:00
|
|
|
//add hit
|
|
|
|
if (rayResult.m_normal.length2() > 0.0001f)
|
|
|
|
{
|
|
|
|
rayResult.m_normal.normalize();
|
|
|
|
if (rayResult.m_fraction < minFraction)
|
|
|
|
{
|
2005-08-17 19:52:56 +00:00
|
|
|
|
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
minFraction = rayResult.m_fraction;
|
|
|
|
nearestHit = ctrl;
|
|
|
|
normalX = rayResult.m_normal.getX();
|
|
|
|
normalY = rayResult.m_normal.getY();
|
|
|
|
normalZ = rayResult.m_normal.getZ();
|
2006-02-21 05:36:56 +00:00
|
|
|
SimdVector3 hitWorld;
|
|
|
|
hitWorld.setInterpolate3(rayFromTrans.getOrigin(),rayToTrans.getOrigin(),rayResult.m_fraction);
|
|
|
|
hitX = hitWorld.getX();
|
|
|
|
hitY = hitWorld.getY();
|
|
|
|
hitZ = hitWorld.getZ();
|
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
}
|
|
|
|
}
|
2005-08-05 17:00:32 +00:00
|
|
|
}
|
|
|
|
}
|
2006-02-13 06:28:35 +00:00
|
|
|
else
|
|
|
|
{
|
|
|
|
if (body->GetCollisionShape()->IsConcave())
|
|
|
|
{
|
2005-08-17 19:52:56 +00:00
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
TriangleMeshShape* triangleMesh = (TriangleMeshShape*)body->GetCollisionShape();
|
|
|
|
|
|
|
|
SimdTransform worldToBody = body->getCenterOfMassTransform().inverse();
|
2005-08-17 19:52:56 +00:00
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
SimdVector3 rayFromLocal = worldToBody * rayFromTrans.getOrigin();
|
|
|
|
SimdVector3 rayToLocal = worldToBody * rayToTrans.getOrigin();
|
2005-08-17 19:52:56 +00:00
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
RaycastCallback rcb(rayFromLocal,rayToLocal);
|
|
|
|
rcb.m_hitFraction = minFraction;
|
2005-08-17 19:52:56 +00:00
|
|
|
|
2006-02-21 05:36:56 +00:00
|
|
|
SimdVector3 rayAabbMinLocal = rayFromLocal;
|
|
|
|
rayAabbMinLocal.setMin(rayToLocal);
|
|
|
|
SimdVector3 rayAabbMaxLocal = rayFromLocal;
|
|
|
|
rayAabbMaxLocal.setMax(rayToLocal);
|
|
|
|
|
|
|
|
triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
|
|
|
|
if (rcb.m_hitFound)
|
2006-02-13 06:28:35 +00:00
|
|
|
{
|
|
|
|
nearestHit = ctrl;
|
|
|
|
minFraction = rcb.m_hitFraction;
|
2006-02-21 05:36:56 +00:00
|
|
|
SimdVector3 hitNormalWorld = body->getCenterOfMassTransform().getBasis()*rcb.m_hitNormalLocal;
|
|
|
|
hitNormalWorld.normalize();
|
2006-02-13 06:28:35 +00:00
|
|
|
|
|
|
|
normalX = hitNormalWorld.getX();
|
|
|
|
normalY = hitNormalWorld.getY();
|
|
|
|
normalZ = hitNormalWorld.getZ();
|
|
|
|
SimdVector3 hitWorld;
|
|
|
|
hitWorld.setInterpolate3(rayFromTrans.getOrigin(),rayToTrans.getOrigin(),rcb.m_hitFraction);
|
|
|
|
hitX = hitWorld.getX();
|
|
|
|
hitY = hitWorld.getY();
|
|
|
|
hitZ = hitWorld.getZ();
|
|
|
|
|
|
|
|
}
|
2005-08-17 19:52:56 +00:00
|
|
|
}
|
2006-02-13 06:28:35 +00:00
|
|
|
}
|
2005-08-17 19:52:56 +00:00
|
|
|
}
|
2005-08-05 17:00:32 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
return nearestHit;
|
2005-07-18 05:41:00 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int CcdPhysicsEnvironment::getNumContactPoints()
|
|
|
|
{
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
|
|
|
|
{
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
2006-02-21 05:36:56 +00:00
|
|
|
BroadphaseInterface* CcdPhysicsEnvironment::GetBroadphase()
|
|
|
|
{
|
|
|
|
return m_collisionWorld->GetBroadphase();
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
const CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher() const
|
|
|
|
{
|
|
|
|
return m_collisionWorld->GetDispatcher();
|
|
|
|
}
|
2005-07-18 05:41:00 +00:00
|
|
|
|
2006-02-21 05:36:56 +00:00
|
|
|
CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher()
|
2005-07-18 05:41:00 +00:00
|
|
|
{
|
2006-02-21 05:36:56 +00:00
|
|
|
return m_collisionWorld->GetDispatcher();
|
2005-07-18 05:41:00 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
|
|
|
|
{
|
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
#ifdef NEW_BULLET_VEHICLE_SUPPORT
|
|
|
|
m_wrapperVehicles.clear();
|
|
|
|
#endif //NEW_BULLET_VEHICLE_SUPPORT
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
//m_broadphase->DestroyScene();
|
|
|
|
//delete broadphase ? release reference on broadphase ?
|
|
|
|
|
|
|
|
//first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
|
2006-02-21 05:36:56 +00:00
|
|
|
//delete m_dispatcher;
|
|
|
|
delete m_collisionWorld;
|
2005-07-18 05:41:00 +00:00
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
int CcdPhysicsEnvironment::GetNumControllers()
|
|
|
|
{
|
|
|
|
return m_controllers.size();
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index)
|
|
|
|
{
|
|
|
|
return m_controllers[index];
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
int CcdPhysicsEnvironment::GetNumManifolds() const
|
|
|
|
{
|
2006-02-21 05:36:56 +00:00
|
|
|
return GetDispatcher()->GetNumManifolds();
|
2005-07-18 05:41:00 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
const PersistentManifold* CcdPhysicsEnvironment::GetManifold(int index) const
|
|
|
|
{
|
2006-02-21 05:36:56 +00:00
|
|
|
return GetDispatcher()->GetManifoldByIndexInternal(index);
|
2005-07-18 05:41:00 +00:00
|
|
|
}
|
2006-02-13 06:28:35 +00:00
|
|
|
|
2006-04-06 20:37:38 +00:00
|
|
|
TypedConstraint* CcdPhysicsEnvironment::getConstraintById(int constraintId)
|
|
|
|
{
|
|
|
|
int numConstraint = m_constraints.size();
|
|
|
|
int i;
|
|
|
|
for (i=0;i<numConstraint;i++)
|
|
|
|
{
|
|
|
|
TypedConstraint* constraint = m_constraints[i];
|
|
|
|
if (constraint->GetUserConstraintId()==constraintId)
|
|
|
|
{
|
|
|
|
return constraint;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return 0;
|
|
|
|
}
|
2006-02-13 06:28:35 +00:00
|
|
|
|
|
|
|
|
2006-04-17 01:33:10 +00:00
|
|
|
void CcdPhysicsEnvironment::addSensor(PHY_IPhysicsController* ctrl)
|
|
|
|
{
|
2006-04-17 06:27:57 +00:00
|
|
|
//printf("addSensor\n");
|
2006-04-17 01:33:10 +00:00
|
|
|
}
|
|
|
|
void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
|
|
|
|
{
|
2006-04-17 06:27:57 +00:00
|
|
|
//printf("removeSensor\n");
|
2006-04-17 01:33:10 +00:00
|
|
|
}
|
|
|
|
void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
|
|
|
|
{
|
2006-04-17 06:27:57 +00:00
|
|
|
/* printf("addTouchCallback\n(response class = %i)\n",response_class);
|
2006-04-17 01:33:10 +00:00
|
|
|
|
|
|
|
//map PHY_ convention into SM_ convention
|
|
|
|
switch (response_class)
|
|
|
|
{
|
|
|
|
case PHY_FH_RESPONSE:
|
|
|
|
printf("PHY_FH_RESPONSE\n");
|
|
|
|
break;
|
|
|
|
case PHY_SENSOR_RESPONSE:
|
|
|
|
printf("PHY_SENSOR_RESPONSE\n");
|
|
|
|
break;
|
|
|
|
case PHY_CAMERA_RESPONSE:
|
|
|
|
printf("PHY_CAMERA_RESPONSE\n");
|
|
|
|
break;
|
|
|
|
case PHY_OBJECT_RESPONSE:
|
|
|
|
printf("PHY_OBJECT_RESPONSE\n");
|
|
|
|
break;
|
|
|
|
case PHY_STATIC_RESPONSE:
|
|
|
|
printf("PHY_STATIC_RESPONSE\n");
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
assert(0);
|
|
|
|
return;
|
|
|
|
}
|
2006-04-17 06:27:57 +00:00
|
|
|
*/
|
2006-04-17 01:33:10 +00:00
|
|
|
|
|
|
|
m_triggerCallbacks[response_class] = callback;
|
|
|
|
m_triggerCallbacksUserPtrs[response_class] = user;
|
|
|
|
|
|
|
|
}
|
|
|
|
void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctrl)
|
|
|
|
{
|
|
|
|
CcdPhysicsController* ccdCtrl = static_cast<CcdPhysicsController*>(ctrl);
|
|
|
|
|
2006-04-17 06:27:57 +00:00
|
|
|
//printf("requestCollisionCallback\n");
|
2006-04-17 01:33:10 +00:00
|
|
|
m_triggerControllers.push_back(ccdCtrl);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void CcdPhysicsEnvironment::CallbackTriggers()
|
|
|
|
{
|
|
|
|
CcdPhysicsController* ctrl0=0,*ctrl1=0;
|
|
|
|
|
|
|
|
if (m_triggerCallbacks[PHY_OBJECT_RESPONSE])
|
|
|
|
{
|
2006-04-17 06:27:57 +00:00
|
|
|
//walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
|
2006-04-17 01:33:10 +00:00
|
|
|
int numManifolds = m_collisionWorld->GetDispatcher()->GetNumManifolds();
|
|
|
|
for (int i=0;i<numManifolds;i++)
|
|
|
|
{
|
|
|
|
PersistentManifold* manifold = m_collisionWorld->GetDispatcher()->GetManifoldByIndexInternal(i);
|
|
|
|
int numContacts = manifold->GetNumContacts();
|
|
|
|
if (numContacts)
|
|
|
|
{
|
|
|
|
RigidBody* obj0 = static_cast<RigidBody* >(manifold->GetBody0());
|
|
|
|
RigidBody* obj1 = static_cast<RigidBody* >(manifold->GetBody1());
|
|
|
|
|
|
|
|
//m_userPointer is set in 'addPhysicsController
|
|
|
|
CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->m_userPointer);
|
|
|
|
CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->m_userPointer);
|
|
|
|
|
|
|
|
std::vector<CcdPhysicsController*>::iterator i =
|
|
|
|
std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl0);
|
|
|
|
if (i == m_triggerControllers.end())
|
|
|
|
{
|
|
|
|
i = std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl1);
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!(i == m_triggerControllers.end()))
|
|
|
|
{
|
|
|
|
m_triggerCallbacks[PHY_OBJECT_RESPONSE](m_triggerCallbacksUserPtrs[PHY_OBJECT_RESPONSE],
|
|
|
|
ctrl0,ctrl1,0);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
}
|
2006-04-17 06:27:57 +00:00
|
|
|
|
2006-04-17 01:33:10 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
2006-02-13 06:28:35 +00:00
|
|
|
#ifdef NEW_BULLET_VEHICLE_SUPPORT
|
|
|
|
|
|
|
|
//complex constraint for vehicles
|
|
|
|
PHY_IVehicle* CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
|
|
|
|
{
|
|
|
|
int i;
|
|
|
|
|
|
|
|
int numVehicles = m_wrapperVehicles.size();
|
|
|
|
for (i=0;i<numVehicles;i++)
|
|
|
|
{
|
|
|
|
WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
|
|
|
|
if (wrapperVehicle->GetVehicle()->GetUserConstraintId() == constraintId)
|
|
|
|
return wrapperVehicle;
|
|
|
|
}
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2006-03-24 16:40:32 +00:00
|
|
|
#endif //NEW_BULLET_VEHICLE_SUPPORT
|
|
|
|
|