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