public static void Multiply(ref btVector3 v, ref btMatrix3x3 m, out btVector3 result)
{
result.X = m.tdotx(ref v);
result.Y = m.tdoty(ref v);
result.Z = m.tdotz(ref v);
result.W = 0;
}
public static void Multiply(ref btMatrix3x3 m, ref btVector3 v, out btVector3 result)
{
//result = new btVector3();
result.X = m.el0.dot(v);
result.Y = m.el1.dot(v);
result.Z = m.el2.dot(v);
result.W = 0;
}
/*public static btMatrix3x3 operator *(btMatrix3x3 m1, btMatrix3x3 m2)
{
return new btMatrix3x3(
m2.tdotx(m1.el0), m2.tdoty(m1.el0), m2.tdotz(m1.el0),
m2.tdotx(m1.el1), m2.tdoty(m1.el1), m2.tdotz(m1.el1),
m2.tdotx(m1.el2), m2.tdoty(m1.el2), m2.tdotz(m1.el2));
}*/
public static void Multiply(ref btMatrix3x3 m1, ref btMatrix3x3 m2, out btMatrix3x3 result)
{
result.el0.X = m2.tdotx(ref m1.el0);
result.el0.Y = m2.tdoty(ref m1.el0);
result.el0.Z = m2.tdotz(ref m1.el0);
result.el0.W = 0;
result.el1.X = m2.tdotx(ref m1.el1);
result.el1.Y = m2.tdoty(ref m1.el1);
result.el1.Z = m2.tdotz(ref m1.el1);
result.el1.W = 0;
result.el2.X = m2.tdotx(ref m1.el2);
result.el2.Y = m2.tdoty(ref m1.el2);
result.el2.Z = m2.tdotz(ref m1.el2);
result.el2.W = 0;
}
public override void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.getNewManifold(body0, body1);
m_ownManifold = true;
}
resultOut.PersistentManifold = m_manifoldPtr;
//comment-out next line to test multi-contact generation
//resultOut->getPersistentManifold()->clearManifold();
ConvexShape min0 = (ConvexShape)(body0.CollisionShape);
ConvexShape min1 = (ConvexShape)(body1.CollisionShape);
btVector3 normalOnB;
btVector3 pointOnBWorld;
if ((min0.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = (CapsuleShape)min0;
CapsuleShape capsuleB = (CapsuleShape)min1;
btVector3 localScalingA = capsuleA.LocalScaling;
btVector3 localScalingB = capsuleB.LocalScaling;
float threshold = m_manifoldPtr.ContactBreakingThreshold;
float dist = capsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.HalfHeight, capsuleA.Radius,
capsuleB.HalfHeight, capsuleB.Radius, capsuleA.UpAxis, capsuleB.UpAxis,
body0.WorldTransform, body1.WorldTransform, threshold);
if (dist < threshold)
{
Debug.Assert(normalOnB.Length2 >= (BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON));
resultOut.addContactPoint(ref normalOnB, ref pointOnBWorld, dist);
}
resultOut.refreshContactPoints();
return;
}
#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;
GjkPairDetector gjkPairDetector = new GjkPairDetector(min0, min1, m_simplexSolver, m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.MinkowskiA = min0;
gjkPairDetector.MinkowskiB = min1;
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
} else
#endif //USE_SEPDISTANCE_UTIL2
{
input.m_maximumDistanceSquared = min0.Margin + min1.Margin + m_manifoldPtr.ContactBreakingThreshold;
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
//input.m_stackAlloc = dispatchInfo.m_stackAllocator;
input.m_transformA = body0.WorldTransform;
input.m_transformB = body1.WorldTransform;
gjkPairDetector.getClosestPoints(ref input, ref resultOut, dispatchInfo.m_debugDraw);
#if USE_SEPDISTANCE_UTIL2
btScalar sepDist = 0.f;
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
sepDist = gjkPairDetector.getCachedSeparatingDistance();
if (sepDist>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.PersistentManifold.NumContacts < m_minimumPointsPerturbationThreshold)
{
int i;
btVector3 v0, v1;
btVector3 sepNormalWorldSpace;
//sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
gjkPairDetector.getCachedSeparatingAxis().normalized(out sepNormalWorldSpace);
btVector3.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1);
bool perturbeA = true;
float angleLimit = 0.125f * BulletGlobal.SIMD_PI;
float perturbeAngle;
float radiusA = min0.getAngularMotionDisc();
float radiusB = min1.getAngularMotionDisc();
if (radiusA < radiusB)
{
perturbeAngle = PersistentManifold.gContactBreakingThreshold / radiusA;
perturbeA = true;
}
else
{
perturbeAngle = PersistentManifold.gContactBreakingThreshold / radiusB;
perturbeA = false;
}
if (perturbeAngle > angleLimit)
perturbeAngle = angleLimit;
btTransform unPerturbedTransform;
if (perturbeA)
{
unPerturbedTransform = input.m_transformA;
}
else
{
unPerturbedTransform = input.m_transformB;
}
for (i = 0; i < m_numPerturbationIterations; i++)
{
if (v0.Length2 > BulletGlobal.SIMD_EPSILON)
{
btQuaternion perturbeRot = new btQuaternion(v0, perturbeAngle);
float iterationAngle = i * (BulletGlobal.SIMD_2_PI / m_numPerturbationIterations);
btQuaternion rotq = new btQuaternion(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
#region input.m_transformA.Basis = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body0.WorldTransform.Basis;
{
btMatrix3x3 temp = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq);
btMatrix3x3.Multiply(ref temp, ref body0.WorldTransform.Basis, out input.m_transformA.Basis);
}
#endregion
input.m_transformB = body1.WorldTransform;
#if DEBUG
dispatchInfo.m_debugDraw.drawTransform(ref input.m_transformA, 10.0f);
#endif //DEBUG_CONTACTS
}
else
{
input.m_transformA = body0.WorldTransform;
#region input.m_transformB.Basis = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body1.WorldTransform.Basis;
{
btMatrix3x3 temp = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq);
btMatrix3x3.Multiply(ref temp, ref body1.WorldTransform.Basis, out input.m_transformB.Basis);
}
#endregion
#if DEBUG
dispatchInfo.m_debugDraw.drawTransform(ref input.m_transformB, 10.0f);
#endif
}
PerturbedContactResult perturbedResultOut = new PerturbedContactResult(input.m_transformA, input.m_transformB, unPerturbedTransform, perturbeA, dispatchInfo.m_debugDraw);
gjkPairDetector.getClosestPoints(ref input, ref perturbedResultOut, ref resultOut, dispatchInfo.m_debugDraw);
}
}
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2
}
if (m_ownManifold)
{
resultOut.refreshContactPoints();
}
}
/*public btMatrix3x3 transposeTimes(btMatrix3x3 m)
{
return new btMatrix3x3(
el0.X * m.el0.X + el1.X * m.el1.X + el2.X * m.el2.X,
el0.X * m.el0.Y + el1.X * m.el1.Y + el2.X * m.el2.Y,
el0.X * m.el0.Z + el1.X * m.el1.Z + el2.X * m.el2.Z,
el0.Y * m.el0.X + el1.Y * m.el1.X + el2.Y * m.el2.X,
el0.Y * m.el0.Y + el1.Y * m.el1.Y + el2.Y * m.el2.Y,
el0.Y * m.el0.Z + el1.Y * m.el1.Z + el2.Y * m.el2.Z,
el0.Z * m.el0.X + el1.Z * m.el1.X + el2.Z * m.el2.X,
el0.Z * m.el0.Y + el1.Z * m.el1.Y + el2.Z * m.el2.Y,
el0.Z * m.el0.Z + el1.Z * m.el1.Z + el2.Z * m.el2.Z);
}*/
public void transposeTimes(ref btMatrix3x3 m, out btMatrix3x3 result)
{
result.el0.X = el0.X * m.el0.X + el1.X * m.el1.X + el2.X * m.el2.X;
result.el0.Y = el0.X * m.el0.Y + el1.X * m.el1.Y + el2.X * m.el2.Y;
result.el0.Z = el0.X * m.el0.Z + el1.X * m.el1.Z + el2.X * m.el2.Z;
result.el0.W = 0;
result.el1.X = el0.Y * m.el0.X + el1.Y * m.el1.X + el2.Y * m.el2.X;
result.el1.Y = el0.Y * m.el0.Y + el1.Y * m.el1.Y + el2.Y * m.el2.Y;
result.el1.Z = el0.Y * m.el0.Z + el1.Y * m.el1.Z + el2.Y * m.el2.Z;
result.el1.W = 0;
result.el2.X = el0.Z * m.el0.X + el1.Z * m.el1.X + el2.Z * m.el2.X;
result.el2.Y = el0.Z * m.el0.Y + el1.Z * m.el1.Y + el2.Z * m.el2.Y;
result.el2.Z = el0.Z * m.el0.Z + el1.Z * m.el1.Z + el2.Z * m.el2.Z;
result.el2.W = 0;
}
/*public btMatrix3x3 inverse()
{
btVector3 co=new btVector3(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
float det = el0.dot(co);
Debug.Assert(det != 0.0);
float s = 1.0f / det;
return new btMatrix3x3(co.X * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
co.Y * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
co.Z * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
}*/
public void inverse(out btMatrix3x3 result)
{
btVector3 co = new btVector3(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
float det = el0.dot(co);
Debug.Assert(det != 0.0);
float s = 1.0f / det;
result = new btMatrix3x3(co.X * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
co.Y * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
co.Z * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
}
/*public btMatrix3x3 absolute()
{
return new btMatrix3x3(
Math.Abs(el0.X), Math.Abs(el0.Y), Math.Abs(el0.Z),
Math.Abs(el1.X), Math.Abs(el1.Y), Math.Abs(el1.Z),
Math.Abs(el2.X), Math.Abs(el2.Y), Math.Abs(el2.Z));
}*/
public void absolute(out btMatrix3x3 result)
{
result.el0.X = Math.Abs(el0.X);
result.el0.Y = Math.Abs(el0.Y);
result.el0.Z = Math.Abs(el0.Z);
result.el0.W = 0;
result.el1.X = Math.Abs(el1.X);
result.el1.Y = Math.Abs(el1.Y);
result.el1.Z = Math.Abs(el1.Z);
result.el1.W = 0;
result.el2.X = Math.Abs(el2.X);
result.el2.Y = Math.Abs(el2.Y);
result.el2.Z = Math.Abs(el2.Z);
result.el2.W = 0;
}
/*public btMatrix3x3 scaled(btVector3 s)
{
return new btMatrix3x3(el0.X * s.X, el0.Y * s.Y, el0.Z * s.Z,
el1.X * s.X, el1.Y * s.Y, el1.Z * s.Z,
el2.X * s.X, el2.Y * s.Y, el2.Z * s.Z);
}*/
public void scaled(ref btVector3 s, out btMatrix3x3 result)
{
result.el0.X = el0.X * s.X;
result.el0.Y = el0.Y * s.Y;
result.el0.Z = el0.Z * s.Z;
result.el0.W = 0;
result.el1.X = el1.X * s.X;
result.el1.Y = el1.Y * s.Y;
result.el1.Z = el1.Z * s.Z;
result.el1.W = 0;
result.el2.X = el2.X * s.X;
result.el2.Y = el2.Y * s.Y;
result.el2.Z = el2.Z * s.Z;
result.el2.W = 0;
}
/*public btMatrix3x3 transpose()
{
return new btMatrix3x3(el0.X, el1.X, el2.X,
el0.Y, el1.Y, el2.Y,
el0.Z, el1.Z, el2.Z);
}*/
public void transpose(out btMatrix3x3 result)
{
result.el0.X = el0.X;
result.el0.Y = el1.X;
result.el0.Z = el2.X;
result.el0.W = 0;
result.el1.X = el0.Y;
result.el1.Y = el1.Y;
result.el1.Z = el2.Y;
result.el1.W = 0;
result.el2.X = el0.Z;
result.el2.Y = el1.Z;
result.el2.Z = el2.Z;
result.el2.W = 0;
}
public void collideSingleContact(btQuaternion perturbeRot, CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
CollisionObject convexObj = m_isSwapped ? body1 : body0;
CollisionObject planeObj = m_isSwapped ? body0 : body1;
ConvexShape convexShape = (ConvexShape)convexObj.CollisionShape;
StaticPlaneShape planeShape = (StaticPlaneShape)planeObj.CollisionShape;
bool hasCollision = false;
btVector3 planeNormal = planeShape.PlaneNormal;
float planeConstant = planeShape.PlaneConstant;
btTransform convexWorldTransform = convexObj.WorldTransform;
btTransform convexInPlaneTrans;
convexInPlaneTrans = planeObj.WorldTransform.inverse() * convexWorldTransform;
//now perturbe the convex-world transform
#region convexWorldTransform.Basis *= new btMatrix3x3(perturbeRot);
{
btMatrix3x3 temp1 = convexWorldTransform.Basis, temp2 = new btMatrix3x3(perturbeRot);
btMatrix3x3.Multiply(ref temp1, ref temp2, out convexWorldTransform.Basis);
}
#endregion
btTransform planeInConvex;
planeInConvex = convexWorldTransform.inverse() * planeObj.WorldTransform;
#region btVector3 vtx = convexShape.localGetSupportingVertex(planeInConvex.Basis * -planeNormal);
btVector3 vtx;
{
btVector3 temp, temp2;
temp2 = -planeNormal;
btMatrix3x3.Multiply(ref planeInConvex.Basis, ref temp2, out temp);
//vtx = convexShape.localGetSupportingVertex(temp);
convexShape.localGetSupportingVertex(ref temp, out vtx);
}
#endregion
btVector3 vtxInPlane = convexInPlaneTrans * vtx;
float distance = (planeNormal.dot(vtxInPlane) - planeConstant);
btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
btVector3 vtxInPlaneWorld = planeObj.WorldTransform * vtxInPlaneProjected;
hasCollision = distance < m_manifoldPtr.ContactBreakingThreshold;
resultOut.PersistentManifold = m_manifoldPtr;
if (hasCollision)
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
btVector3 normalOnSurfaceB;// = planeObj.WorldTransform.Basis * planeNormal;
btMatrix3x3.Multiply(ref planeObj.WorldTransform.Basis, ref planeNormal, out normalOnSurfaceB);
btVector3 pOnB = vtxInPlaneWorld;
resultOut.addContactPoint(ref normalOnSurfaceB, ref pOnB, distance);
}
}
