public override void InitializeDemo() { // Setup the basic world SetTexturing(true); SetShadows(true); SetCameraDistance(5.0f); m_collisionConfiguration = new DefaultCollisionConfiguration(); m_dispatcher = new CollisionDispatcher(m_collisionConfiguration); IndexedVector3 worldAabbMin = new IndexedVector3(-10000,-10000,-10000); IndexedVector3 worldAabbMax = new IndexedVector3(10000,10000,10000); //m_broadphase = new AxisSweep3Internal(ref worldAabbMin, ref worldAabbMax, 0xfffe, 0xffff, 16384, null, true); m_broadphase = new SimpleBroadphase(1000, null); m_constraintSolver = new SequentialImpulseConstraintSolver(); m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration); //m_dynamicsWorld.getDispatchInfo().m_useConvexConservativeDistanceUtil = true; //m_dynamicsWorld.getDispatchInfo().m_convexConservativeDistanceThreshold = 0.01f; // Setup a big ground box { CollisionShape groundShape = new BoxShape(new IndexedVector3(200.0f,10.0f,200.0f)); m_collisionShapes.Add(groundShape); IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(0,-10,0); CollisionObject fixedGround = new CollisionObject(); fixedGround.SetCollisionShape(groundShape); fixedGround.SetWorldTransform(ref groundTransform); fixedGround.SetUserPointer("Ground"); m_dynamicsWorld.AddCollisionObject(fixedGround); } // Spawn one ragdoll IndexedVector3 startOffset = new IndexedVector3(1,0.5f,0); //string filename = @"c:\users\man\bullet\xna-ragdoll-constraints-output.txt"; //FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read); //BulletGlobals.g_streamWriter = new StreamWriter(filestream); SpawnRagdoll(ref startOffset, BulletGlobals.g_streamWriter); //startOffset = new IndexedVector3(-1,0.5f,0); //spawnRagdoll(ref startOffset); ClientResetScene(); }
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { if (m_manifoldPtr == null) { return; } CollisionObject convexObj = m_isSwapped ? body1 : body0; CollisionObject planeObj = m_isSwapped ? body0 : body1; ConvexShape convexShape = convexObj.GetCollisionShape() as ConvexShape; if (convexShape == null) { convexShape = new BoxShape(IndexedVector3.One * 0.5f); body0.SetCollisionShape(convexShape); } StaticPlaneShape planeShape = planeObj.GetCollisionShape() as StaticPlaneShape; if (planeShape == null) { planeShape = new StaticPlaneShape(IndexedVector3.Up, 9f); } bool hasCollision = false; if (convexShape == null || planeShape == null) { //resultOut = null; return; } IndexedVector3 planeNormal = planeShape.GetPlaneNormal(); float planeConstant = planeShape.GetPlaneConstant(); IndexedMatrix planeInConvex; planeInConvex = convexObj.GetWorldTransform().Inverse() * planeObj.GetWorldTransform(); IndexedMatrix convexInPlaneTrans; convexInPlaneTrans = planeObj.GetWorldTransform().Inverse() * convexObj.GetWorldTransform(); IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal); IndexedVector3 vtxInPlane = convexInPlaneTrans * vtx; float distance = (planeNormal.Dot(vtxInPlane) - planeConstant); IndexedVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal; IndexedVector3 vtxInPlaneWorld = planeObj.GetWorldTransform() * vtxInPlaneProjected; hasCollision = distance < m_manifoldPtr.GetContactBreakingThreshold(); resultOut.SetPersistentManifold(m_manifoldPtr); if (hasCollision) { /// report a contact. internally this will be kept persistent, and contact reduction is done IndexedVector3 normalOnSurfaceB = planeObj.GetWorldTransform()._basis *planeNormal; IndexedVector3 pOnB = vtxInPlaneWorld; resultOut.AddContactPoint(normalOnSurfaceB, pOnB, distance); } ////first perform a collision query with the non-perturbated collision objects //{ // IndexedQuaternion rotq = IndexedQuaternion.Identity; // CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut); //} if (convexShape.IsPolyhedral() && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold) { IndexedVector3 v0; IndexedVector3 v1; TransformUtil.PlaneSpace1(ref planeNormal, out v0, out v1); //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects float angleLimit = 0.125f * MathUtil.SIMD_PI; float perturbeAngle; float radius = convexShape.GetAngularMotionDisc(); perturbeAngle = BulletGlobals.gContactBreakingThreshold / radius; if (perturbeAngle > angleLimit) { perturbeAngle = angleLimit; } IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle); for (int i = 0; i < m_numPerturbationIterations; i++) { float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations); IndexedQuaternion rotq = new IndexedQuaternion(planeNormal, iterationAngle); rotq = IndexedQuaternion.Inverse(rotq) * perturbeRot * rotq; CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut); } } if (m_ownManifold) { if (m_manifoldPtr.GetNumContacts() > 0) { resultOut.RefreshContactPoints(); } } }