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);
}
}
