From 5102b7ac60cebad20b2ec04e16efd845eb8a09e8 Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Thu, 9 Nov 2006 04:43:18 +0000 Subject: [PATCH] added very basic debug drawing for vehicle wheels, and updated raycast in world to do ray-aabb instead of aabb-aabb --- Demos/VehicleDemo/VehicleDemo.cpp | 88 ++++++++++------- .../CollisionDispatch/btCollisionWorld.cpp | 16 +-- .../Dynamics/btDiscreteDynamicsWorld.cpp | 99 +++++++++++++------ src/LinearMath/btAabbUtil2.h | 1 + 4 files changed, 124 insertions(+), 80 deletions(-) diff --git a/Demos/VehicleDemo/VehicleDemo.cpp b/Demos/VehicleDemo/VehicleDemo.cpp index afbcf01aa..1d3d7915d 100644 --- a/Demos/VehicleDemo/VehicleDemo.cpp +++ b/Demos/VehicleDemo/VehicleDemo.cpp @@ -34,17 +34,17 @@ const int maxOverlap = 65535; ///notice that for higher-quality slow-moving vehicles, another approach might be better ///implementing explicit hinged-wheel constraints with cylinder collision, rather then raycasts float gEngineForce = 0.f; -float maxEngineForce = 100.f; +float maxEngineForce = 3000.f; float gVehicleSteering = 0.f; -float steeringIncrement = 0.1f; +float steeringIncrement = 0.04f; float steeringClamp = 0.3f; float wheelRadius = 0.5f; -float wheelWidth = 0.2f; -float wheelFriction = 100.f; -float suspensionStiffness = 10.f; -float suspensionDamping = 1.3f; -float suspensionCompression = 2.4f; -float rollInfluence = 0.1f; +float wheelWidth = 0.4f; +float wheelFriction = 1e30f;//1000;//1e30f; +float suspensionStiffness = 20.f; +float suspensionDamping = 2.3f; +float suspensionCompression = 4.4f; +float rollInfluence = 0.1f;//1.0f; btVector3 wheelDirectionCS0(0,-1,0); btVector3 wheelAxleCS(1,0,0); btScalar suspensionRestLength(0.6); @@ -76,6 +76,7 @@ m_cameraHeight(4.f), m_minCameraDistance(3.f), m_maxCameraDistance(10.f) { + m_vehicle = 0; m_cameraPosition = btVector3(30,30,30); } @@ -86,6 +87,7 @@ void VehicleDemo::setupPhysics() btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50)); m_dynamicsWorld = new btDiscreteDynamicsWorld(); + m_dynamicsWorld->setDebugDrawer(&debugDrawer); #define USE_TRIMESH_GROUND 1 @@ -113,7 +115,9 @@ const float TRIANGLE_SIZE=20.f; { for (int j=0;jaddChildShape(localTrans,chassisShape); + tr.setOrigin(btVector3(0,0.f,0)); - m_carChassis = localCreateRigidBody(800,tr,chassisShape); + m_carChassis = localCreateRigidBody(800,tr,compound);//chassisShape); + m_carChassis->setDamping(0.2,0.2); + clientResetScene(); @@ -167,46 +180,34 @@ const float TRIANGLE_SIZE=20.f; m_dynamicsWorld->addVehicle(m_vehicle); + float connectionHeight = 1.2f; + - btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),0,2*CUBE_HALF_EXTENTS-wheelRadius); + btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius); bool isFrontWheel=true; int rightIndex = 0; int upIndex = 1; int forwardIndex = 2; m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex); - m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel); - connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),0,2*CUBE_HALF_EXTENTS-wheelRadius); + connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius); m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel); - connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),0,-2*CUBE_HALF_EXTENTS+wheelRadius); + connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius); isFrontWheel = false; m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel); - connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),0,-2*CUBE_HALF_EXTENTS+wheelRadius); + connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius); m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel); -/* gVehicleConstraint->SetSuspensionStiffness(suspensionStiffness,0); - gVehicleConstraint->SetSuspensionStiffness(suspensionStiffness,1); - gVehicleConstraint->SetSuspensionStiffness(suspensionStiffness,2); - gVehicleConstraint->SetSuspensionStiffness(suspensionStiffness,3); - - gVehicleConstraint->SetSuspensionDamping(suspensionDamping,0); - gVehicleConstraint->SetSuspensionDamping(suspensionDamping,1); - gVehicleConstraint->SetSuspensionDamping(suspensionDamping,2); - gVehicleConstraint->SetSuspensionDamping(suspensionDamping,3); - - gVehicleConstraint->SetSuspensionCompression(suspensionCompression,0); - gVehicleConstraint->SetSuspensionCompression(suspensionCompression,1); - gVehicleConstraint->SetSuspensionCompression(suspensionCompression,2); - gVehicleConstraint->SetSuspensionCompression(suspensionCompression,3); - - gVehicleConstraint->SetWheelFriction(wheelFriction,0); - gVehicleConstraint->SetWheelFriction(wheelFriction,1); - gVehicleConstraint->SetWheelFriction(wheelFriction,2); - gVehicleConstraint->SetWheelFriction(wheelFriction,3); - */ - - + for (int i=0;igetNumWheels();i++) + { + btWheelInfo& wheel = m_vehicle->getWheelInfo(i); + wheel.m_suspensionStiffness = suspensionStiffness; + wheel.m_wheelsDampingRelaxation = suspensionDamping; + wheel.m_wheelsDampingCompression = suspensionCompression; + wheel.m_frictionSlip = wheelFriction; + wheel.m_rollInfluence = rollInfluence; + } } @@ -338,6 +339,19 @@ void VehicleDemo::clientResetScene() gEngineForce = 0.f; gVehicleSteering = 0.f; m_carChassis->setCenterOfMassTransform(btTransform::getIdentity()); + m_carChassis->setLinearVelocity(btVector3(0,0,0)); + m_carChassis->setAngularVelocity(btVector3(0,0,0)); + m_dynamicsWorld->getBroadphase()->cleanProxyFromPairs(m_carChassis->getBroadphaseHandle()); + if (m_vehicle) + { + m_vehicle->resetSuspension(); + for (int i=0;igetNumWheels();i++) + { + //synchronize the wheels with the (interpolated) chassis worldtransform + m_vehicle->updateWheelTransform(i); + } + } + } diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 537dcd464..7edd3e8ab 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -320,15 +320,7 @@ void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r rayToTrans.setOrigin(rayToWorld); - //do culling based on aabb (rayFrom/rayTo) - btVector3 rayAabbMin = rayFromWorld; - btVector3 rayAabbMax = rayFromWorld; - rayAabbMin.setMin(rayToWorld); - rayAabbMax.setMax(rayToWorld); - - - /// brute force go over all objects. Once there is a broadphase, use that, or - /// add a raycast against aabb first. + /// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD) std::vector::iterator iter; @@ -342,11 +334,9 @@ void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r btVector3 collisionObjectAabbMin,collisionObjectAabbMax; collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); - //check aabb overlap - - float hitLambda = 1.f; + float hitLambda = 1.f; //could use resultCallback.m_closestHitFraction, but needs testing btVector3 hitNormal; - if (btRayAabb(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) + if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) { rayTestSingle(rayFromTrans,rayToTrans, collisionObject, diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index d605198c6..8c6ee72c0 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -118,45 +118,84 @@ void btDiscreteDynamicsWorld::saveKinematicState(float timeStep) void btDiscreteDynamicsWorld::synchronizeMotionStates() { - //todo: iterate over awake simulation islands! - for (unsigned int i=0;igetDebugMode() & btIDebugDraw::DBG_DrawWireframe) { - btCollisionObject* colObj = m_collisionObjects[i]; - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) + for (unsigned int i=0;im_vehicles.size();i++) { - btVector3 color(255.f,255.f,255.f); - switch(colObj->getActivationState()) + for (int v=0;vgetNumWheels();v++) { - case ACTIVE_TAG: - color = btVector3(255.f,255.f,255.f); break; - case ISLAND_SLEEPING: - color = btVector3(0.f,255.f,0.f);break; - case WANTS_DEACTIVATION: - color = btVector3(0.f,255.f,255.f);break; - case DISABLE_DEACTIVATION: - color = btVector3(255.f,0.f,0.f);break; - case DISABLE_SIMULATION: - color = btVector3(255.f,255.f,0.f);break; - default: + btVector3 wheelColor(0,255,255); + if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact) { - color = btVector3(255.f,0.f,0.f); + wheelColor.setValue(0,0,255); + } else + { + wheelColor.setValue(255,0,255); } - }; - debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); - } - btRigidBody* body = btRigidBody::upcast(colObj); - if (body && body->getMotionState() && !body->isStaticOrKinematicObject()) - { - if (body->getActivationState() != ISLAND_SLEEPING) - { - btTransform interpolatedTransform; - btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), - body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform); - body->getMotionState()->setWorldTransform(interpolatedTransform); + //synchronize the wheels with the (interpolated) chassis worldtransform + m_vehicles[i]->updateWheelTransform(v); + + btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin(); + + btVector3 axle = btVector3( + m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()], + m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()], + m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]); + + + //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS + //debug wheels (cylinders) + m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor); + m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor); + } } } + { + //todo: iterate over awake simulation islands! + for (unsigned int i=0;igetDebugMode() & btIDebugDraw::DBG_DrawWireframe) + { + btVector3 color(255.f,255.f,255.f); + switch(colObj->getActivationState()) + { + case ACTIVE_TAG: + color = btVector3(255.f,255.f,255.f); break; + case ISLAND_SLEEPING: + color = btVector3(0.f,255.f,0.f);break; + case WANTS_DEACTIVATION: + color = btVector3(0.f,255.f,255.f);break; + case DISABLE_DEACTIVATION: + color = btVector3(255.f,0.f,0.f);break; + case DISABLE_SIMULATION: + color = btVector3(255.f,255.f,0.f);break; + default: + { + color = btVector3(255.f,0.f,0.f); + } + }; + + debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); + } + btRigidBody* body = btRigidBody::upcast(colObj); + if (body && body->getMotionState() && !body->isStaticOrKinematicObject()) + { + if (body->getActivationState() != ISLAND_SLEEPING) + { + btTransform interpolatedTransform; + btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), + body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform); + body->getMotionState()->setWorldTransform(interpolatedTransform); + } + } + } + } + } diff --git a/src/LinearMath/btAabbUtil2.h b/src/LinearMath/btAabbUtil2.h index eecad3be5..2eacb8e24 100644 --- a/src/LinearMath/btAabbUtil2.h +++ b/src/LinearMath/btAabbUtil2.h @@ -66,6 +66,7 @@ SIMD_FORCE_INLINE int btOutcode(const btVector3& p,const btVector3& halfExtent) (p.getZ() > halfExtent.getZ() ? 0x20 : 0x0); } + SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, const btVector3& rayTo, const btVector3& aabbMin,