From f55e45f855250a5693455fe0f7e13fe0f35c7814 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 13 Feb 2006 06:28:35 +0000 Subject: more vehicle preparation and some bullet collision detection bugfixes (related to collision margin) --- source/gameengine/Ketsji/KX_KetsjiEngine.cpp | 19 + source/gameengine/Ketsji/KX_VehicleWrapper.cpp | 150 ++++++- source/gameengine/Ketsji/KX_VehicleWrapper.h | 18 +- .../Physics/Bullet/CcdPhysicsEnvironment.cpp | 441 +++++++++++++++++---- .../Physics/Bullet/CcdPhysicsEnvironment.h | 11 +- .../gameengine/Physics/common/PHY_DynamicTypes.h | 1 - source/gameengine/Physics/common/PHY_IVehicle.h | 16 +- 7 files changed, 576 insertions(+), 80 deletions(-) (limited to 'source') diff --git a/source/gameengine/Ketsji/KX_KetsjiEngine.cpp b/source/gameengine/Ketsji/KX_KetsjiEngine.cpp index 8be153ecb60..e9d10f785b0 100644 --- a/source/gameengine/Ketsji/KX_KetsjiEngine.cpp +++ b/source/gameengine/Ketsji/KX_KetsjiEngine.cpp @@ -333,6 +333,13 @@ void KX_KetsjiEngine::NextFrame() m_clockTime = m_kxsystem->GetTimeInSeconds(); double deltatime = m_clockTime - m_frameTime; + if (deltatime<0.f) + { + printf("problem with clock\n"); + deltatime = 0.f; + m_clockTime = 0.f; + m_frameTime = 0.f; + } // Compute the number of logic frames to do each update (fixed tic bricks) int frames =int(deltatime*m_ticrate); @@ -341,9 +348,21 @@ void KX_KetsjiEngine::NextFrame() // PIL_sleep_ms(4); KX_SceneList::iterator sceneit; + int frameOut = 5; + if (frames>frameOut) + { + printf("framedOut: %d\n",frames); + m_frameTime+=(frames-frameOut)*(1.0/m_ticrate); + frames = frameOut; + } + while (frames) { + if (frames > frameOut) + { + printf ("what happened\n"); + } m_frameTime += 1.0/m_ticrate; diff --git a/source/gameengine/Ketsji/KX_VehicleWrapper.cpp b/source/gameengine/Ketsji/KX_VehicleWrapper.cpp index 299a2d1f180..9745af028d5 100644 --- a/source/gameengine/Ketsji/KX_VehicleWrapper.cpp +++ b/source/gameengine/Ketsji/KX_VehicleWrapper.cpp @@ -4,6 +4,9 @@ #include "KX_VehicleWrapper.h" #include "PHY_IPhysicsEnvironment.h" #include "PHY_IVehicle.h" +#include "KX_PyMath.h" +#include "KX_GameObject.h" +#include "KX_MotionState.h" #ifdef HAVE_CONFIG_H #include @@ -20,6 +23,13 @@ KX_VehicleWrapper::KX_VehicleWrapper( KX_VehicleWrapper::~KX_VehicleWrapper() { + int numMotion = m_motionStates.size(); + for (int i=0;iGetSGNode()); + + MT_Vector3 attachPos,attachDir,attachAxle; + PyVecTo(pylistPos,attachPos); + PyVecTo(pylistDir,attachDir); + PyVecTo(pylistAxleDir,attachAxle); + PHY__Vector3 aPos,aDir,aAxle; + aPos[0] = attachPos[0]; + aPos[1] = attachPos[1]; + aPos[2] = attachPos[2]; + aDir[0] = attachDir[0]; + aDir[1] = attachDir[1]; + aDir[2] = attachDir[2]; + aAxle[0] = attachAxle[0]; + aAxle[1] = attachAxle[1]; + aAxle[2] = attachAxle[2]; + + printf("attempt for addWheel: suspensionRestLength%f wheelRadius %f, hasSteering:%d\n",suspensionRestLength,wheelRadius,hasSteering); + m_vehicle->AddWheel(motionState,aPos,aDir,aAxle,suspensionRestLength,wheelRadius,hasSteering); + + } Py_INCREF(Py_None); return Py_None; } -PyObject* KX_VehicleWrapper::PyGetWheelsTransform(PyObject* self, + +PyObject* KX_VehicleWrapper::PyGetWheelPosition(PyObject* self, PyObject* args, PyObject* kwds) { - assert(0); - return PyInt_FromLong(m_vehicle->GetNumWheels()); + + int wheelIndex; + + if (PyArg_ParseTuple(args,"i",&wheelIndex)) + { + float position[3]; + m_vehicle->GetWheelPosition(wheelIndex,position[0],position[1],position[2]); + MT_Vector3 pos(position[0],position[1],position[2]); + return PyObjectFrom(pos); + } + Py_INCREF(Py_None); + return Py_None; +} + +PyObject* KX_VehicleWrapper::PyGetWheelRotation(PyObject* self, + PyObject* args, + PyObject* kwds) +{ + int wheelIndex; + if (PyArg_ParseTuple(args,"i",&wheelIndex)) + { + return PyFloat_FromDouble(m_vehicle->GetWheelRotation(wheelIndex)); + } + Py_INCREF(Py_None); + return Py_None; +} + +PyObject* KX_VehicleWrapper::PyGetWheelOrientationQuaternion(PyObject* self, + PyObject* args, + PyObject* kwds) +{ + int wheelIndex; + if (PyArg_ParseTuple(args,"i",&wheelIndex)) + { + float orn[4]; + m_vehicle->GetWheelOrientationQuaternion(wheelIndex,orn[0],orn[1],orn[2],orn[3]); + MT_Quaternion quatorn(orn[0],orn[1],orn[2],orn[3]); + MT_Matrix3x3 ornmat(quatorn); + return PyObjectFrom(ornmat); + } + Py_INCREF(Py_None); + return Py_None; + } @@ -57,6 +141,58 @@ PyObject* KX_VehicleWrapper::PyGetConstraintId(PyObject* self, return PyInt_FromLong(m_vehicle->GetUserConstraintId()); } + + +PyObject* KX_VehicleWrapper::PyApplyEngineForce(PyObject* self, + PyObject* args, + PyObject* kwds) +{ + float force; + int wheelIndex; + + if (PyArg_ParseTuple(args,"fi",&force,&wheelIndex)) + { + m_vehicle->ApplyEngineForce(force,wheelIndex); + } + Py_INCREF(Py_None); + return Py_None; +} + + +PyObject* KX_VehicleWrapper::PyApplyBraking(PyObject* self, + PyObject* args, + PyObject* kwds) +{ + float braking; + int wheelIndex; + + if (PyArg_ParseTuple(args,"fi",&braking,&wheelIndex)) + { + m_vehicle->ApplyBraking(braking,wheelIndex); + } + Py_INCREF(Py_None); + return Py_None; +} + + + + +PyObject* KX_VehicleWrapper::PySetSteeringValue(PyObject* self, + PyObject* args, + PyObject* kwds) +{ + float steeringValue; + int wheelIndex; + + if (PyArg_ParseTuple(args,"fi",&steeringValue,&wheelIndex)) + { + m_vehicle->SetSteeringValue(steeringValue,wheelIndex); + } + Py_INCREF(Py_None); + return Py_None; +} + + PyObject* KX_VehicleWrapper::PyGetConstraintType(PyObject* self, PyObject* args, PyObject* kwds) @@ -67,6 +203,7 @@ PyObject* KX_VehicleWrapper::PyGetConstraintType(PyObject* self, + //python specific stuff PyTypeObject KX_VehicleWrapper::Type = { PyObject_HEAD_INIT(&PyType_Type) @@ -130,9 +267,14 @@ int KX_VehicleWrapper::_setattr(const STR_String& attr,PyObject* pyobj) PyMethodDef KX_VehicleWrapper::Methods[] = { {"addWheel",(PyCFunction) KX_VehicleWrapper::sPyAddWheel, METH_VARARGS}, {"getNumWheels",(PyCFunction) KX_VehicleWrapper::sPyGetNumWheels, METH_VARARGS}, - {"getWheelsTransform",(PyCFunction) KX_VehicleWrapper::sPyGetWheelsTransform, METH_VARARGS}, + {"getWheelOrientationQuaternion",(PyCFunction) KX_VehicleWrapper::sPyGetWheelOrientationQuaternion, METH_VARARGS}, + {"getWheelRotation",(PyCFunction) KX_VehicleWrapper::sPyGetWheelRotation, METH_VARARGS}, + {"getWheelPosition",(PyCFunction) KX_VehicleWrapper::sPyGetWheelPosition, METH_VARARGS}, {"getConstraintId",(PyCFunction) KX_VehicleWrapper::sPyGetConstraintId, METH_VARARGS}, {"getConstraintType",(PyCFunction) KX_VehicleWrapper::sPyGetConstraintType, METH_VARARGS}, + {"setSteeringValue",(PyCFunction) KX_VehicleWrapper::sPySetSteeringValue, METH_VARARGS}, + {"applyEngineForce",(PyCFunction) KX_VehicleWrapper::sPyApplyEngineForce, METH_VARARGS}, + {"applyBraking",(PyCFunction) KX_VehicleWrapper::sPyApplyBraking, METH_VARARGS}, {NULL,NULL} //Sentinel }; diff --git a/source/gameengine/Ketsji/KX_VehicleWrapper.h b/source/gameengine/Ketsji/KX_VehicleWrapper.h index ebdf1b0fa2f..22b8f186f4a 100644 --- a/source/gameengine/Ketsji/KX_VehicleWrapper.h +++ b/source/gameengine/Ketsji/KX_VehicleWrapper.h @@ -4,6 +4,9 @@ #include "Value.h" #include "PHY_DynamicTypes.h" class PHY_IVehicle; +class PHY_IMotionState; + +#include ///Python interface to physics vehicles (primarily 4-wheel cars and 2wheel bikes) class KX_VehicleWrapper : public PyObjectPlus @@ -11,6 +14,9 @@ class KX_VehicleWrapper : public PyObjectPlus Py_Header; virtual PyObject* _getattr(const STR_String& attr); virtual int _setattr(const STR_String& attr, PyObject *value); + + std::vector m_motionStates; + public: KX_VehicleWrapper(PHY_IVehicle* vehicle,class PHY_IPhysicsEnvironment* physenv,PyTypeObject *T = &Type); virtual ~KX_VehicleWrapper (); @@ -19,11 +25,21 @@ public: KX_PYMETHOD(KX_VehicleWrapper,AddWheel); KX_PYMETHOD(KX_VehicleWrapper,GetNumWheels); - KX_PYMETHOD(KX_VehicleWrapper,GetWheelsTransform); + KX_PYMETHOD(KX_VehicleWrapper,GetWheelOrientationQuaternion); + KX_PYMETHOD(KX_VehicleWrapper,GetWheelRotation); + + KX_PYMETHOD(KX_VehicleWrapper,GetWheelPosition); KX_PYMETHOD(KX_VehicleWrapper,GetConstraintId); KX_PYMETHOD(KX_VehicleWrapper,GetConstraintType); + KX_PYMETHOD(KX_VehicleWrapper,SetSteeringValue); + + KX_PYMETHOD(KX_VehicleWrapper,ApplyEngineForce); + + KX_PYMETHOD(KX_VehicleWrapper,ApplyBraking); + + private: PHY_IVehicle* m_vehicle; diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp index cdf617f1722..afc2aa2c70f 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp +++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp @@ -20,7 +20,7 @@ #include "NarrowPhaseCollision/SubSimplexConvexCast.h" #include "CollisionDispatch/ToiContactDispatcher.h" - +#include "PHY_IMotionState.h" #include "CollisionDispatch/EmptyCollisionAlgorithm.h" #include "CollisionDispatch/UnionFind.h" @@ -30,6 +30,16 @@ bool useIslands = true; +#ifdef NEW_BULLET_VEHICLE_SUPPORT +#include "Vehicle/RaycastVehicle.h" +#include "Vehicle/VehicleRaycaster.h" +#include "Vehicle/VehicleTuning.h" +#include "PHY_IVehicle.h" +VehicleTuning gTuning; + +#endif //NEW_BULLET_VEHICLE_SUPPORT +#include "AabbUtil2.h" + #include "ConstraintSolver/ConstraintSolver.h" #include "ConstraintSolver/Point2PointConstraint.h" //#include "BroadphaseCollision/QueryDispatcher.h" @@ -44,9 +54,146 @@ void DrawRasterizerLine(const float* from,const float* to,int color); #include "ConstraintSolver/ContactConstraint.h" - #include +#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;iGetWheelInfo(i); + PHY_IMotionState* motionState = (PHY_IMotionState*)info.m_clientInfo ; + 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; + } + } + +}; +#endif //NEW_BULLET_VEHICLE_SUPPORT @@ -192,7 +339,7 @@ void CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctr if ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() || (&p2p->GetRigidBodyB() == ctrl->GetRigidBody()))) { - removeConstraint(p2p->GetConstraintId()); + removeConstraint(p2p->GetUserConstraintId()); //only 1 constraint per constroller break; } @@ -209,7 +356,7 @@ void CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctr if ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() || (&p2p->GetRigidBodyB() == ctrl->GetRigidBody()))) { - removeConstraint(p2p->GetConstraintId()); + removeConstraint(p2p->GetUserConstraintId()); //only 1 constraint per constroller break; } @@ -414,20 +561,25 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) p2p->SolveConstraint( timeStep ); } - /* + +#ifdef NEW_BULLET_VEHICLE_SUPPORT //vehicles - int numVehicles = m_vehicles.size(); - for (i=0;iUpdateVehicle( timeStep ); + WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i]; + RaycastVehicle* vehicle = wrapperVehicle->GetVehicle(); + vehicle->UpdateVehicle( timeStep); } - */ +#endif //NEW_BULLET_VEHICLE_SUPPORT + } + + { @@ -593,8 +745,26 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) } + + SyncMotionStates(timeStep); + +#ifdef NEW_BULLET_VEHICLE_SUPPORT + //sync wheels for vehicles + int numVehicles = m_wrapperVehicles.size(); + for (int i=0;iGetVehicle()->GetNumWheels();j++) + { + wrapperVehicle->GetVehicle()->UpdateWheelTransformsWS(wrapperVehicle->GetVehicle()->GetWheelInfo(j)); + } + + wrapperVehicle->SyncWheels(); + } +#endif //NEW_BULLET_VEHICLE_SUPPORT } return true; } @@ -734,6 +904,78 @@ void CcdPhysicsEnvironment::setGravity(float x,float y,float z) } +#ifdef NEW_BULLET_VEHICLE_SUPPORT + +class BlenderVehicleRaycaster : public VehicleRaycaster +{ + CcdPhysicsEnvironment* m_physEnv; + PHY_IPhysicsController* m_chassis; + +public: + BlenderVehicleRaycaster(CcdPhysicsEnvironment* physEnv,PHY_IPhysicsController* chassis): + 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; + 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) @@ -769,12 +1011,32 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl } m_p2pConstraints.push_back(p2p); - + p2p->SetUserConstraintId(gConstraintUid++); + p2p->SetUserConstraintType(type); //64 bit systems can't cast pointer to int. could use size_t instead. - return p2p->GetConstraintId(); + return p2p->GetUserConstraintId(); break; } +#ifdef NEW_BULLET_VEHICLE_SUPPORT + + case PHY_VEHICLE_CONSTRAINT: + { + VehicleTuning* tuning = new VehicleTuning(); + RigidBody* chassis = rb0; + BlenderVehicleRaycaster* raycaster = new BlenderVehicleRaycaster(this,ctrl0); + RaycastVehicle* vehicle = new RaycastVehicle(*tuning,chassis,raycaster); + vehicle->SetBalance(false); + 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 + default: { } @@ -797,7 +1059,7 @@ void CcdPhysicsEnvironment::removeConstraint(int constraintId) !(i==m_p2pConstraints.end()); i++) { Point2PointConstraint* p2p = (*i); - if (p2p->GetConstraintId() == constraintId) + if (p2p->GetUserConstraintId() == constraintId) { std::swap(*i, m_p2pConstraints.back()); m_p2pConstraints.pop_back(); @@ -814,11 +1076,21 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i SimdTransform rayFromTrans,rayToTrans; rayFromTrans.setIdentity(); - rayFromTrans.setOrigin(SimdVector3(fromX,fromY,fromZ)); + + SimdVector3 rayFrom(fromX,fromY,fromZ); + + rayFromTrans.setOrigin(rayFrom); rayToTrans.setIdentity(); - rayToTrans.setOrigin(SimdVector3(toX,toY,toZ)); + SimdVector3 rayTo(toX,toY,toZ); + rayToTrans.setOrigin(rayTo); + //do culling based on aabb (rayFrom/rayTo) + SimdVector3 rayAabbMin = rayFrom; + SimdVector3 rayAabbMax = rayFrom; + rayAabbMin.setMin(rayTo); + rayAabbMax.setMax(rayTo); + CcdPhysicsController* nearestHit = 0; std::vector::iterator i; @@ -830,71 +1102,81 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i !(i==m_controllers.end()); i++) { CcdPhysicsController* ctrl = (*i); + if (ctrl == ignoreClient) + continue; RigidBody* body = ctrl->GetRigidBody(); + SimdVector3 bodyAabbMin,bodyAabbMax; + body->getAabb(bodyAabbMin,bodyAabbMax); - if (body->GetCollisionShape()->IsConvex()) - { - ConvexCast::CastResult rayResult; - rayResult.m_fraction = 1.f; + //check aabb overlap - ConvexShape* convexShape = (ConvexShape*) body->GetCollisionShape(); - VoronoiSimplexSolver simplexSolver; - SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); - - if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,body->getCenterOfMassTransform(),body->getCenterOfMassTransform(),rayResult)) + if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,bodyAabbMin,bodyAabbMax)) + { + if (body->GetCollisionShape()->IsConvex()) { - //add hit - rayResult.m_normal.normalize(); - if (rayResult.m_fraction < minFraction) + ConvexCast::CastResult rayResult; + rayResult.m_fraction = 1.f; + + ConvexShape* convexShape = (ConvexShape*) body->GetCollisionShape(); + VoronoiSimplexSolver simplexSolver; + SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); + + if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,body->getCenterOfMassTransform(),body->getCenterOfMassTransform(),rayResult)) { + //add hit + if (rayResult.m_normal.length2() > 0.0001f) + { + rayResult.m_normal.normalize(); + if (rayResult.m_fraction < minFraction) + { - minFraction = rayResult.m_fraction; - nearestHit = ctrl; - normalX = rayResult.m_normal.getX(); - normalY = rayResult.m_normal.getY(); - normalZ = rayResult.m_normal.getZ(); - hitX = rayResult.m_hitTransformA.getOrigin().getX(); - hitY = rayResult.m_hitTransformA.getOrigin().getY(); - hitZ = rayResult.m_hitTransformA.getOrigin().getZ(); + minFraction = rayResult.m_fraction; + nearestHit = ctrl; + normalX = rayResult.m_normal.getX(); + normalY = rayResult.m_normal.getY(); + normalZ = rayResult.m_normal.getZ(); + hitX = rayResult.m_hitTransformA.getOrigin().getX(); + hitY = rayResult.m_hitTransformA.getOrigin().getY(); + hitZ = rayResult.m_hitTransformA.getOrigin().getZ(); + } + } } } - } - else - { - if (body->GetCollisionShape()->IsConcave()) - { - - TriangleMeshShape* triangleMesh = (TriangleMeshShape*)body->GetCollisionShape(); - - SimdTransform worldToBody = body->getCenterOfMassTransform().inverse(); + else + { + if (body->GetCollisionShape()->IsConcave()) + { - SimdVector3 rayFromLocal = worldToBody * rayFromTrans.getOrigin(); - SimdVector3 rayToLocal = worldToBody * rayToTrans.getOrigin(); + TriangleMeshShape* triangleMesh = (TriangleMeshShape*)body->GetCollisionShape(); + + SimdTransform worldToBody = body->getCenterOfMassTransform().inverse(); - RaycastCallback rcb(rayFromLocal,rayToLocal); - rcb.m_hitFraction = minFraction; + SimdVector3 rayFromLocal = worldToBody * rayFromTrans.getOrigin(); + SimdVector3 rayToLocal = worldToBody * rayToTrans.getOrigin(); - SimdVector3 aabbMax(1e30f,1e30f,1e30f); + RaycastCallback rcb(rayFromLocal,rayToLocal); + rcb.m_hitFraction = minFraction; - triangleMesh->ProcessAllTriangles(&rcb,-aabbMax,aabbMax); - if (rcb.m_hitFound)// && (rcb.m_hitFraction < minFraction)) - { - nearestHit = ctrl; - minFraction = rcb.m_hitFraction; - SimdVector3 hitNormalWorld = body->getCenterOfMassTransform()(rcb.m_hitNormalLocal); - - 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(); - + triangleMesh->ProcessAllTriangles(&rcb,rayAabbMin,rayAabbMax); + if (rcb.m_hitFound)// && (rcb.m_hitFraction < minFraction)) + { + nearestHit = ctrl; + minFraction = rcb.m_hitFraction; + SimdVector3 hitNormalWorld = body->getCenterOfMassTransform()(rcb.m_hitNormalLocal); + + 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(); + + } } - } + } } } @@ -925,8 +1207,9 @@ Dispatcher* CcdPhysicsEnvironment::GetDispatcher() CcdPhysicsEnvironment::~CcdPhysicsEnvironment() { - - m_vehicles.clear(); +#ifdef NEW_BULLET_VEHICLE_SUPPORT + m_wrapperVehicles.clear(); +#endif //NEW_BULLET_VEHICLE_SUPPORT //m_broadphase->DestroyScene(); //delete broadphase ? release reference on broadphase ? @@ -958,3 +1241,25 @@ const PersistentManifold* CcdPhysicsEnvironment::GetManifold(int index) const { return m_dispatcher->GetManifoldByIndexInternal(index); } + + + +#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;iGetVehicle()->GetUserConstraintId() == constraintId) + return wrapperVehicle; + } + + return 0; +} + +#endif //NEW_BULLET_VEHICLE_SUPPORT \ No newline at end of file diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h index a599968a305..9664e9b6233 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h +++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h @@ -13,7 +13,10 @@ class ToiContactDispatcher; class Dispatcher; //#include "BroadphaseInterface.h" -class Vehicle; +//switch on/off new vehicle support +//#define NEW_BULLET_VEHICLE_SUPPORT 1 + +class WrapperVehicle; class PersistentManifold; class BroadphaseInterface; class IDebugDraw; @@ -79,11 +82,15 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment float axisX,float axisY,float axisZ); virtual void removeConstraint(int constraintid); +#ifdef NEW_BULLET_VEHICLE_SUPPORT //complex constraint for vehicles + virtual PHY_IVehicle* getVehicleConstraint(int constraintId); +#else virtual PHY_IVehicle* getVehicleConstraint(int constraintId) { return 0; } +#endif //NEW_BULLET_VEHICLE_SUPPORT virtual PHY_IPhysicsController* 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); @@ -132,7 +139,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment std::vector m_p2pConstraints; - std::vector m_vehicles; + std::vector m_wrapperVehicles; class ToiContactDispatcher* m_dispatcher; diff --git a/source/gameengine/Physics/common/PHY_DynamicTypes.h b/source/gameengine/Physics/common/PHY_DynamicTypes.h index e247a8c5f2a..54aaf240660 100644 --- a/source/gameengine/Physics/common/PHY_DynamicTypes.h +++ b/source/gameengine/Physics/common/PHY_DynamicTypes.h @@ -103,7 +103,6 @@ typedef enum PHY_ConstraintType { } PHY_ConstraintType; -typedef float PHY_Vector3[3]; #endif //__PHY_DYNAMIC_TYPES diff --git a/source/gameengine/Physics/common/PHY_IVehicle.h b/source/gameengine/Physics/common/PHY_IVehicle.h index 655c1154bb9..0e6fe3379a2 100644 --- a/source/gameengine/Physics/common/PHY_IVehicle.h +++ b/source/gameengine/Physics/common/PHY_IVehicle.h @@ -13,9 +13,10 @@ public: virtual ~PHY_IVehicle(); virtual void AddWheel( - PHY_Vector3 connectionPoint, - PHY_Vector3 downDirection, - PHY_Vector3 axleDirection, + PHY_IMotionState* motionState, + PHY__Vector3 connectionPoint, + PHY__Vector3 downDirection, + PHY__Vector3 axleDirection, float suspensionRestLength, float wheelRadius, bool hasSteering @@ -24,13 +25,20 @@ public: virtual int GetNumWheels() const = 0; - virtual const PHY_IMotionState* GetWheelMotionState(int wheelIndex) const = 0; + virtual void GetWheelPosition(int wheelIndex,float& posX,float& posY,float& posZ) const = 0; + virtual void GetWheelOrientationQuaternion(int wheelIndex,float& quatX,float& quatY,float& quatZ,float& quatW) const = 0; + virtual float GetWheelRotation(int wheelIndex) const = 0; virtual int GetUserConstraintId() const =0; virtual int GetUserConstraintType() const =0; //some basic steering/braking/tuning/balancing (bikes) + virtual void SetSteeringValue(float steering,int wheelIndex) = 0; + + virtual void ApplyEngineForce(float force,int wheelIndex) = 0; + + virtual void ApplyBraking(float braking,int wheelIndex) = 0; }; -- cgit v1.2.3