public float GetMarginNonVirtual() { switch (m_shapeType) { case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE: { SphereShape sphereShape = this as SphereShape; return(sphereShape.GetRadius()); } case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE: { BoxShape convexShape = this as BoxShape; return(convexShape.GetMarginNV()); } case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE: { TriangleShape triangleShape = this as TriangleShape; return(triangleShape.GetMarginNV()); } case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE: { CylinderShape cylShape = this as CylinderShape; return(cylShape.GetMarginNV()); } case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE: { CapsuleShape capsuleShape = this as CapsuleShape; return(capsuleShape.GetMarginNV()); } case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: /* fall through */ case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE: { PolyhedralConvexShape convexHullShape = this as PolyhedralConvexShape; return(convexHullShape.GetMarginNV()); } default: return(this.GetMargin()); } // should never reach here Debug.Assert(false); return(0.0f); }
public IndexedVector3 LocalGetSupportVertexWithoutMarginNonVirtual(ref IndexedVector3 localDir) { singleResult = IndexedVector3.Zero; if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexShape) { BulletGlobals.g_streamWriter.WriteLine("localGetSupportVertexWithoutMarginNonVirtual " + GetName()); } switch (m_shapeType) { case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE: { singleResult = new IndexedVector3(); break; } case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE: { BoxShape convexShape = this as BoxShape; IndexedVector3 halfExtents = convexShape.GetImplicitShapeDimensions(); singleResult = new IndexedVector3(MathUtil.FSel(localDir.X, halfExtents.X, -halfExtents.X), MathUtil.FSel(localDir.Y, halfExtents.Y, -halfExtents.Y), MathUtil.FSel(localDir.Z, halfExtents.Z, -halfExtents.Z)); if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexShape) { BulletGlobals.g_streamWriter.WriteLine("localGetSupportVertexWithoutMarginNonVirtual::Box"); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "halfEx", halfExtents); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "localDir", localDir); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "result", singleResult); } break; } case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE: { TriangleShape triangleShape = (TriangleShape)this; IndexedVector3 dir = localDir; IndexedVector3[] vertices = triangleShape.m_vertices1; IndexedVector3 dots = new IndexedVector3(IndexedVector3.Dot(ref dir, ref vertices[0]), IndexedVector3.Dot(ref dir, ref vertices[1]), IndexedVector3.Dot(ref dir, ref vertices[2])); int maxAxis = MathUtil.MaxAxis(ref dots); IndexedVector3 sup = vertices[maxAxis]; if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexShape) { BulletGlobals.g_streamWriter.WriteLine("localGetSupportVertexWithoutMarginNonVirtual::Triangle"); BulletGlobals.g_streamWriter.WriteLine(String.Format("MaxAxis [{0}]", maxAxis)); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "vtx0", vertices[0]); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "vtx1", vertices[1]); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "vtx2", vertices[2]); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "dir", dir); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "dots", dots); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sup", sup); } singleResult = sup; break; } case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE: { CylinderShape cylShape = (CylinderShape)this; //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) IndexedVector3 halfExtents = cylShape.GetImplicitShapeDimensions(); IndexedVector3 v = localDir; int cylinderUpAxis = cylShape.GetUpAxis(); int XX = 1; int YY = 0; int ZZ = 2; switch (cylinderUpAxis) { case 0: { XX = 1; YY = 0; ZZ = 2; } break; case 1: { XX = 0; YY = 1; ZZ = 2; } break; case 2: { XX = 0; YY = 2; ZZ = 1; } break; default: Debug.Assert(false); break; } ; float radius = halfExtents[XX]; float halfHeight = halfExtents[cylinderUpAxis]; IndexedVector3 tmp = new IndexedVector3(); float d; float vx = v[XX]; float vz = v[ZZ]; float s = (float)Math.Sqrt(vx * vx + vz * vz); if (s != 0f) { d = radius / s; tmp[XX] = v[XX] * d; tmp[YY] = v[YY] < 0.0f ? -halfHeight : halfHeight; tmp[ZZ] = v[ZZ] * d; singleResult = tmp; } else { tmp[XX] = radius; tmp[YY] = v[YY] < 0.0f ? -halfHeight : halfHeight; tmp[ZZ] = 0.0f; singleResult = tmp; } break; } case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE: { IndexedVector3 vec0 = localDir; CapsuleShape capsuleShape = this as CapsuleShape; float halfHeight = capsuleShape.GetHalfHeight(); int capsuleUpAxis = capsuleShape.GetUpAxis(); float radius = capsuleShape.GetRadius(); IndexedVector3 supVec = new IndexedVector3(); float maxDot = float.MinValue; IndexedVector3 vec = vec0; float lenSqr = vec.LengthSquared(); if (lenSqr < 0.0001f) { vec = new IndexedVector3(1, 0, 0); } else { float rlen = (1.0f) / (float)Math.Sqrt(lenSqr); vec *= rlen; //vec = IndexedVector3.Normalize(vec); } IndexedVector3 vtx; float newDot; { IndexedVector3 pos = new IndexedVector3(); pos[capsuleUpAxis] = halfHeight; //vtx = pos +vec*(radius); vtx = pos + vec * (radius) - vec * capsuleShape.GetMarginNV(); newDot = IndexedVector3.Dot(ref vec, ref vtx); if (newDot > maxDot) { maxDot = newDot; supVec = vtx; } } { IndexedVector3 pos = new IndexedVector3(); pos[capsuleUpAxis] = -halfHeight; //vtx = pos +vec*(radius); vtx = pos + vec * (radius) - vec * capsuleShape.GetMarginNV(); newDot = IndexedVector3.Dot(ref vec, ref vtx); if (newDot > maxDot) { maxDot = newDot; supVec = vtx; } } singleResult = supVec; break; } case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: { ConvexPointCloudShape convexPointCloudShape = (ConvexPointCloudShape)this; IList <IndexedVector3> points = convexPointCloudShape.GetUnscaledPoints(); int numPoints = convexPointCloudShape.GetNumPoints(); IndexedVector3 localScaling = convexPointCloudShape.GetLocalScalingNV(); singleResult = ConvexHullSupport(ref localDir, points, numPoints, ref localScaling); break; } case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE: { ConvexHullShape convexHullShape = (ConvexHullShape)this; IList <IndexedVector3> points = convexHullShape.GetUnscaledPoints(); int numPoints = convexHullShape.GetNumPoints(); IndexedVector3 localScaling = convexHullShape.GetLocalScalingNV(); singleResult = ConvexHullSupport(ref localDir, points, numPoints, ref localScaling); break; } default: singleResult = LocalGetSupportingVertexWithoutMargin(ref localDir); break; } // should never reach here //Debug.Assert(false); if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexShape) { BulletGlobals.g_streamWriter.WriteLine("localGetSupportVertexWithoutMarginNonVirtual"); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "localDir", localDir); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "result", singleResult); } return(singleResult); }
public virtual void GetAabbNonVirtual(ref IndexedMatrix t, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax) { switch (m_shapeType) { case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE: { SphereShape sphereShape = this as SphereShape; float radius = sphereShape.GetImplicitShapeDimensions().X;// * convexShape->getLocalScaling().getX(); float margin = radius + sphereShape.GetMarginNonVirtual(); IndexedVector3 center = t._origin; IndexedVector3 extent = new IndexedVector3(margin); aabbMin = center - extent; aabbMax = center + extent; } break; case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE: /* fall through */ case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE: { BoxShape convexShape = this as BoxShape; float margin = convexShape.GetMarginNonVirtual(); IndexedVector3 halfExtents = convexShape.GetImplicitShapeDimensions(); halfExtents += new IndexedVector3(margin); IndexedBasisMatrix abs_b = t._basis.Absolute(); IndexedVector3 center = t._origin; IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents), abs_b._el1.Dot(ref halfExtents), abs_b._el2.Dot(ref halfExtents)); aabbMin = center - extent; aabbMax = center + extent; break; } case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE: { TriangleShape triangleShape = (TriangleShape)this; float margin = triangleShape.GetMarginNonVirtual(); for (int i = 0; i < 3; i++) { IndexedVector3 vec = new IndexedVector3(); vec[i] = 1f; IndexedVector3 sv = LocalGetSupportVertexWithoutMarginNonVirtual(vec * t._basis); IndexedVector3 tmp = t * sv; aabbMax[i] = tmp[i] + margin; vec[i] = -1.0f; tmp = t * (LocalGetSupportVertexWithoutMarginNonVirtual(vec * t._basis)); aabbMin[i] = tmp[i] - margin; } } break; case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE: { CapsuleShape capsuleShape = this as CapsuleShape; float r = capsuleShape.GetRadius(); IndexedVector3 halfExtents = new IndexedVector3(r); int m_upAxis = capsuleShape.GetUpAxis(); halfExtents[m_upAxis] = r + capsuleShape.GetHalfHeight(); float nvMargin = capsuleShape.GetMarginNonVirtual(); halfExtents += new IndexedVector3(nvMargin); IndexedBasisMatrix abs_b = t._basis.Absolute(); IndexedVector3 center = t._origin; IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents), abs_b._el1.Dot(ref halfExtents), abs_b._el2.Dot(ref halfExtents)); aabbMin = center - extent; aabbMax = center + extent; } break; case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE: { PolyhedralConvexAabbCachingShape convexHullShape = (PolyhedralConvexAabbCachingShape)this; float margin = convexHullShape.GetMarginNonVirtual(); convexHullShape.GetNonvirtualAabb(ref t, out aabbMin, out aabbMax, margin); } break; default: GetAabb(ref t, out aabbMin, out aabbMax); break; } // should never reach here Debug.Assert(false); }
public override void InitializeDemo() { base.InitializeDemo(); SetCameraDistance(50f); //string filename = @"E:\users\man\bullet\xna-basic-output-1.txt"; //FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read); //BulletGlobals.g_streamWriter = new StreamWriter(filestream); ///collision configuration contains default setup for memory, collision setup m_collisionConfiguration = new DefaultCollisionConfiguration(); ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) m_dispatcher = new CollisionDispatcher(m_collisionConfiguration); IndexedVector3 worldMin = new IndexedVector3 (-1000,-1000,-1000); IndexedVector3 worldMax = -worldMin; m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false); //pairCache = new SortedOverlappingPairCache(); //m_broadphase = new SimpleBroadphase(1000, pairCache); ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) SequentialImpulseConstraintSolver sol = new SequentialImpulseConstraintSolver(); m_constraintSolver = sol; m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration); m_dynamicsWorld.GetDispatchInfo().SetAllowedCcdPenetration(0.0001f); IndexedVector3 gravity = new IndexedVector3(0, -10, 0); m_dynamicsWorld.SetGravity(ref gravity); ///create a few basic rigid bodies IndexedVector3 halfExtents = new IndexedVector3(50, 50, 50); //IndexedVector3 halfExtents = new IndexedVector3(10, 10, 10); CollisionShape groundShape = new BoxShape(ref halfExtents); //CollisionShape groundShape = new StaticPlaneShape(new IndexedVector3(0,1,0), 50); m_collisionShapes.Add(groundShape); IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, -50, 0)); //IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0,-10,0)); float mass = 0f; LocalCreateRigidBody(mass, ref groundTransform, groundShape); #region CharacterController IndexedMatrix startTransform = IndexedMatrix.Identity; //startTransform.setOrigin (btVector3(0.0, 4.0, 0.0)); startTransform._origin = new IndexedVector3(10.210098f,-1.6433364f,16.453260f); m_ghostObject = new PairCachingGhostObject(); m_ghostObject.SetWorldTransform(startTransform); m_broadphase.GetOverlappingPairCache().SetInternalGhostPairCallback(new GhostPairCallback()); float characterHeight=1.75f; float characterWidth =1.75f; ConvexShape capsule = new CapsuleShape(characterWidth,characterHeight); m_ghostObject.SetCollisionShape (capsule); m_ghostObject.SetCollisionFlags (CollisionFlags.CF_CHARACTER_OBJECT); float stepHeight = 0.35f; int upAxis = 1; m_character = new KinematicCharacterController (m_ghostObject,capsule,stepHeight,upAxis); m_dynamicsWorld.AddCollisionObject(m_ghostObject, CollisionFilterGroups.CharacterFilter, CollisionFilterGroups.StaticFilter | CollisionFilterGroups.DefaultFilter); m_dynamicsWorld.AddAction(m_character); #endregion }
public virtual void LoadPlayerController(Entity playerEntity, SceneNode characterNode, object userData, Vector3 mobNodePositionUpdate) { //if (!initialized) //{ // characterToLoad = characterNode; // characterEntityToLoad = playerEntity; // JumpHandlerToLoad = jumpHandler; // MobNodePositionUpdateToLoad = mobNodePositionUpdate; // userDataToLoad = userData; // return; //} if (playerController != null) return; float modelHeight = 2f;// (playerEntity.BoundingBox.Max.Y) / scaleFactor; // AJ: used to subtract minimum from maximum- playerEntity.BoundingBox.Minimum.y System.Console.WriteLine("Player capsule info: modelheight '{0}', boundingbox max '{1}', bounding box min '{2}' and playerPosition '{3}'", modelHeight, playerEntity.BoundingBox.Max.Y, playerEntity.BoundingBox.Min.Y, characterNode.Position); float radius = 1.75f; float height = 1.75f; ConvexShape capsule = new CapsuleShape(radius, height); //ConvexShape capsule = new SphereShape(radius); ghostObject = new PairCachingGhostObject(); Vector3 position = new Vector3(0, 0, 0);//new Vector3(characterNode.Position.X / scaleFactor, (characterNode.Position.Y + 1500) / scaleFactor, characterNode.Position.Z / scaleFactor); //IndexedMatrix worldTransform = IndexedMatrix.CreateTranslation(characterNode.Position.X / scaleFactor, // (characterNode.Position.Y + 1500) / scaleFactor, characterNode.Position.Z / scaleFactor); IndexedMatrix worldTransform = IndexedMatrix.CreateTranslation(position); ghostObject.SetWorldTransform(worldTransform); //broadphase.OverlappingPairCache.SetInternalGhostPairCallback(new GhostPairCallback()); ghostObject.SetCollisionShape(capsule); ghostObject.SetCollisionFlags(CollisionFlags.CF_CHARACTER_OBJECT); float stepHeight = 0.35f; playerController = new KinematicCharacterController(ghostObject, capsule, stepHeight, 1); //characterToLoad = null; BulletMobState mobMovementState = new BulletMobState(playerController, mobNodePositionUpdate); //mobMovementState.JumpEvent += jumpHandler; mobControllers.Add(characterNode, mobMovementState); //m_dynamicsWorld.AddCollisionObject(ghostObject, CollisionFilterGroups.CharacterFilter, CollisionFilterGroups.StaticFilter | CollisionFilterGroups.DefaultFilter); m_dynamicsWorld.AddCollisionObject(ghostObject, CollisionFilterGroups.CharacterFilter, CollisionFilterGroups.StaticFilter | CollisionFilterGroups.DefaultFilter); //m_dynamicsWorld.AddCollisionObject(ghostObject, CollisionFilterGroups.DefaultFilter, CollisionFilterGroups.AllFilter); m_dynamicsWorld.AddAction(playerController); //collisionShapes.Add(capsule); //frozenTime = 0; }
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { if (m_manifoldPtr == null) { //swapped? m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1); m_ownManifold = true; } //resultOut = new ManifoldResult(); resultOut.SetPersistentManifold(m_manifoldPtr); //comment-out next line to test multi-contact generation //resultOut.GetPersistentManifold().ClearManifold(); ConvexShape min0 = body0.GetCollisionShape() as ConvexShape; ConvexShape min1 = body1.GetCollisionShape() as ConvexShape; IndexedVector3 normalOnB; IndexedVector3 pointOnBWorld; #if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER if ((min0.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE)) { CapsuleShape capsuleA = min0 as CapsuleShape; CapsuleShape capsuleB = min1 as CapsuleShape; //IndexedVector3 localScalingA = capsuleA.GetLocalScaling(); //IndexedVector3 localScalingB = capsuleB.GetLocalScaling(); float threshold = m_manifoldPtr.GetContactBreakingThreshold(); float dist = CapsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.GetHalfHeight(), capsuleA.GetRadius(), capsuleB.GetHalfHeight(), capsuleB.GetRadius(), capsuleA.GetUpAxis(), capsuleB.GetUpAxis(), body0.GetWorldTransform(), body1.GetWorldTransform(), threshold); if (dist < threshold) { Debug.Assert(normalOnB.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)); resultOut.AddContactPoint(ref normalOnB, ref pointOnBWorld, dist); } resultOut.RefreshContactPoints(); return; } #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(), body1.getWorldTransform()); } if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance() <= 0.f) #endif //USE_SEPDISTANCE_UTIL2 { ClosestPointInput input = ClosestPointInput.Default(); using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get()) { gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver); //TODO: if (dispatchInfo.m_useContinuous) gjkPairDetector.SetMinkowskiA(min0); gjkPairDetector.SetMinkowskiB(min1); #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { input.m_maximumDistanceSquared = float.MaxValue; } else #endif //USE_SEPDISTANCE_UTIL2 { input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold(); input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared; } //input.m_stackAlloc = dispatchInfo.m_stackAllocator; input.m_transformA = body0.GetWorldTransform(); input.m_transformB = body1.GetWorldTransform(); if (min0.IsPolyhedral() && min1.IsPolyhedral()) { DummyResult dummy = new DummyResult(); PolyhedralConvexShape polyhedronA = min0 as PolyhedralConvexShape; PolyhedralConvexShape polyhedronB = min1 as PolyhedralConvexShape; if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetConvexPolyhedron() != null) { float threshold = m_manifoldPtr.GetContactBreakingThreshold(); float minDist = float.MinValue; IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0); bool foundSepAxis = true; if (dispatchInfo.m_enableSatConvex) { foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis( polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(), body0.GetWorldTransform(), body1.GetWorldTransform(), out sepNormalWorldSpace); } else { #if ZERO_MARGIN gjkPairDetector.SetIgnoreMargin(true); gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw); #else gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw); #endif float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared(); if (l2 > MathUtil.SIMD_EPSILON) { sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2); //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin(); #if ZERO_MARGIN foundSepAxis = true; //gjkPairDetector.getCachedSeparatingDistance()<0.f; #else foundSepAxis = gjkPairDetector.GetCachedSeparatingDistance() < (min0.GetMargin() + min1.GetMargin()); #endif } } if (foundSepAxis) { // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); PolyhedralContactClipping.ClipHullAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(), body0.GetWorldTransform(), body1.GetWorldTransform(), minDist - threshold, threshold, resultOut); } if (m_ownManifold) { resultOut.RefreshContactPoints(); } return; } else { //we can also deal with convex versus triangle (without connectivity data) if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE) { m_vertices.Clear(); TriangleShape tri = polyhedronB as TriangleShape; m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[0]); m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[1]); m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[2]); float threshold = m_manifoldPtr.GetContactBreakingThreshold(); IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);; float minDist = float.MinValue; float maxDist = threshold; bool foundSepAxis = false; if (false) { polyhedronB.InitializePolyhedralFeatures(); foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis( polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(), body0.GetWorldTransform(), body1.GetWorldTransform(), out sepNormalWorldSpace); // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); } else { #if ZERO_MARGIN gjkPairDetector.SetIgnoreMargin(true); gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw); #else gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw); #endif//ZERO_MARGIN float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared(); if (l2 > MathUtil.SIMD_EPSILON) { sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2); //minDist = gjkPairDetector.getCachedSeparatingDistance(); //maxDist = threshold; minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin(); foundSepAxis = true; } } if (foundSepAxis) { PolyhedralContactClipping.ClipFaceAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), body0.GetWorldTransform(), m_vertices, minDist - threshold, maxDist, resultOut); } if (m_ownManifold) { resultOut.RefreshContactPoints(); } return; } } } gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false); #if USE_SEPDISTANCE_UTIL2 float sepDist = 0.f; if (dispatchInfo.m_useConvexConservativeDistanceUtil) { sepDist = gjkPairDetector.getCachedSeparatingDistance(); if (sepDist > MathUtil.SIMD_EPSILON) { sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; //now perturbe directions to get multiple contact points } } #endif //USE_SEPDISTANCE_UTIL2 //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points if (m_numPerturbationIterations > 0 && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold) { IndexedVector3 v0, v1; IndexedVector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis(); sepNormalWorldSpace.Normalize(); TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1); bool perturbeA = true; const float angleLimit = 0.125f * MathUtil.SIMD_PI; float perturbeAngle; float radiusA = min0.GetAngularMotionDisc(); float radiusB = min1.GetAngularMotionDisc(); if (radiusA < radiusB) { perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusA; perturbeA = true; } else { perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusB; perturbeA = false; } if (perturbeAngle > angleLimit) { perturbeAngle = angleLimit; } IndexedMatrix unPerturbedTransform; if (perturbeA) { unPerturbedTransform = input.m_transformA; } else { unPerturbedTransform = input.m_transformB; } for (int i = 0; i < m_numPerturbationIterations; i++) { if (v0.LengthSquared() > MathUtil.SIMD_EPSILON) { IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle); float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations); IndexedQuaternion rotq = new IndexedQuaternion(sepNormalWorldSpace, iterationAngle); if (perturbeA) { input.m_transformA._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body0.GetWorldTransform()._basis); input.m_transformB = body1.GetWorldTransform(); input.m_transformB = body1.GetWorldTransform(); #if DEBUG_CONTACTS dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformA, 10.0f); #endif //DEBUG_CONTACTS } else { input.m_transformA = body0.GetWorldTransform(); input.m_transformB._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body1.GetWorldTransform()._basis); #if DEBUG_CONTACTS dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformB, 10.0f); #endif } PerturbedContactResult perturbedResultOut = new PerturbedContactResult(resultOut, ref input.m_transformA, ref input.m_transformB, ref unPerturbedTransform, perturbeA, dispatchInfo.getDebugDraw()); gjkPairDetector.GetClosestPoints(ref input, perturbedResultOut, dispatchInfo.getDebugDraw(), false); } } } #if USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > MathUtil.SIMD_EPSILON)) { m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(), sepDist, body0.getWorldTransform(), body1.getWorldTransform()); } #endif //USE_SEPDISTANCE_UTIL2 } } if (m_ownManifold) { resultOut.RefreshContactPoints(); } }
public TestRig (DemoApplication demoApplication,DynamicsWorld ownerWorld, ref IndexedVector3 positionOffset, bool bFixed) { m_dynamicsWorld = ownerWorld; IndexedVector3 vUp = new IndexedVector3(0, 1, 0); // // Setup geometry // float fBodySize = 0.25f; float fLegLength = 0.45f; float fForeLegLength = 0.75f; m_shapes[0] = new CapsuleShape(fBodySize, 0.10f); for (int i=0; i<NUM_LEGS; i++) { m_shapes[1 + 2*i] = new CapsuleShape(0.10f, fLegLength); m_shapes[2 + 2*i] = new CapsuleShape(0.08f, fForeLegLength); } // // Setup rigid bodies // float fHeight = 0.5f; IndexedMatrix offset = IndexedMatrix.Identity; offset._origin = positionOffset; // root IndexedVector3 vRoot = new IndexedVector3(0,fHeight,0); IndexedMatrix transform = IndexedMatrix.Identity; transform._origin = vRoot; if (bFixed) { m_bodies[0] = demoApplication.LocalCreateRigidBody(0.0f, offset * transform, m_shapes[0]); } else { m_bodies[0] = demoApplication.LocalCreateRigidBody(1.0f, offset * transform, m_shapes[0]); } // legs for (int i=0; i<NUM_LEGS; i++) { float fAngle = MathUtil.SIMD_2_PI * i / NUM_LEGS; float fSin = (float)Math.Sin(fAngle); float fCos = (float)Math.Cos(fAngle); transform = IndexedMatrix.Identity; IndexedVector3 vBoneOrigin = new IndexedVector3(fCos*(fBodySize+0.5f*fLegLength), fHeight, fSin*(fBodySize+0.5f*fLegLength)); transform._origin = vBoneOrigin; // thigh IndexedVector3 vToBone = (vBoneOrigin - vRoot); vToBone.Normalize(); IndexedVector3 vAxis = IndexedVector3.Cross(vToBone,vUp); transform._basis = new IndexedBasisMatrix(Quaternion.CreateFromAxisAngle(vAxis.ToVector3(), MathUtil.SIMD_HALF_PI)); transform._origin = vBoneOrigin; m_bodies[1+2*i] = demoApplication.LocalCreateRigidBody(1.0f, offset * transform, m_shapes[1+2*i]); // shin transform = IndexedMatrix.Identity; transform._origin = new IndexedVector3(fCos*(fBodySize+fLegLength), fHeight-0.5f*fForeLegLength, fSin*(fBodySize+fLegLength)); m_bodies[2+2*i] = demoApplication.LocalCreateRigidBody(1.0f, offset * transform, m_shapes[2+2*i]); } // Setup some damping on the m_bodies for (int i = 0; i < BODYPART_COUNT; ++i) { m_bodies[i].SetDamping(0.05f, 0.85f); m_bodies[i].SetDeactivationTime(0.8f); //m_bodies[i].setSleepingThresholds(1.6, 2.5); m_bodies[i].SetSleepingThresholds(0.5f, 0.5f); } // // Setup the constraints // HingeConstraint hingeC; IndexedMatrix localA, localB, localC; for (int i=0; i<NUM_LEGS; i++) { float fAngle = MathUtil.SIMD_2_PI * i / NUM_LEGS; float fSin = (float)Math.Sin(fAngle); float fCos = (float)Math.Cos(fAngle); // hip joints localA = IndexedMatrix.Identity; localB= IndexedMatrix.Identity; localA = MathUtil.SetEulerZYX(0f,-fAngle,0f); localA._origin = new IndexedVector3(fCos*fBodySize, 0.0f, fSin*fBodySize); localB = m_bodies[1 + 2 * i].GetWorldTransform().Inverse() * m_bodies[0].GetWorldTransform() * localA; if (BulletGlobals.g_streamWriter != null && false) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Hip LocalA", localA); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Hip LocalB", localB); } hingeC = new HingeConstraint(m_bodies[0], m_bodies[1+2*i], ref localA, ref localB); hingeC.SetLimit(-0.75f * MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI/2f); m_joints[2*i] = hingeC; m_dynamicsWorld.AddConstraint(m_joints[2*i], true); // knee joints localA = IndexedMatrix.Identity; localB= IndexedMatrix.Identity; localC = IndexedMatrix.Identity; localA = MathUtil.SetEulerZYX(0f,-fAngle,0f); localA._origin = new IndexedVector3(fCos*(fBodySize+fLegLength), 0.0f, fSin*(fBodySize+fLegLength)); localB = m_bodies[1 + 2 * i].GetWorldTransform().Inverse() * m_bodies[0].GetWorldTransform() * localA; localC = m_bodies[2 + 2 * i].GetWorldTransform().Inverse() * m_bodies[0].GetWorldTransform() * localA; if (BulletGlobals.g_streamWriter != null && false) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Knee LocalA", localA); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Knee LocalB", localB); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Knee LocalC", localC); } hingeC = new HingeConstraint(m_bodies[1 + 2 * i], m_bodies[2 + 2 * i], ref localB, ref localC); //hingeC.setLimit(float(-0.01), float(0.01)); hingeC.SetLimit(-MathUtil.SIMD_QUARTER_PI/2f, 0.2f); m_joints[1+2*i] = hingeC; m_dynamicsWorld.AddConstraint(m_joints[1+2*i], true); } }