public static void GetPlaneEquationTransformed(StaticPlaneShape plane, ref IndexedMatrix trans, out IndexedVector4 equation)
{
equation = new IndexedVector4();
IndexedVector3 planeNormal = plane.GetPlaneNormal();
equation.X = trans._basis.GetRow(0).Dot(ref planeNormal);
equation.Y = trans._basis.GetRow(1).Dot(ref planeNormal);
equation.Z = trans._basis.GetRow(2).Dot(ref planeNormal);
equation.W = trans._origin.Dot(ref planeNormal) + plane.GetPlaneConstant();
}
public static void GetPlaneEquationTransformed(StaticPlaneShape plane,ref IndexedMatrix trans, out IndexedVector4 equation)
{
equation = new IndexedVector4();
IndexedVector3 planeNormal = plane.GetPlaneNormal();
equation.X = trans._basis.GetRow(0).Dot(ref planeNormal);
equation.Y = trans._basis.GetRow(1).Dot(ref planeNormal);
equation.Z = trans._basis.GetRow(2).Dot(ref planeNormal);
equation.W = trans._origin.Dot(ref planeNormal) + plane.GetPlaneConstant();
}
public virtual void CollideSingleContact(ref IndexedQuaternion perturbeRot, CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject convexObj = m_isSwapped ? body1 : body0;
CollisionObject planeObj = m_isSwapped ? body0 : body1;
ConvexShape convexShape = convexObj.GetCollisionShape() as ConvexShape;
StaticPlaneShape planeShape = planeObj.GetCollisionShape() as StaticPlaneShape;
bool hasCollision = false;
IndexedVector3 planeNormal = planeShape.GetPlaneNormal();
float planeConstant = planeShape.GetPlaneConstant();
IndexedMatrix convexWorldTransform = convexObj.GetWorldTransform();
IndexedMatrix convexInPlaneTrans = planeObj.GetWorldTransform().Inverse() * convexWorldTransform;
//now perturbe the convex-world transform
convexWorldTransform._basis *= new IndexedBasisMatrix(ref perturbeRot);
IndexedMatrix planeInConvex = convexWorldTransform.Inverse() * planeObj.GetWorldTransform();;
IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);
IndexedVector3 vtxInPlane = vtxInPlane = convexInPlaneTrans * vtx;
float distance = (IndexedVector3.Dot(planeNormal, 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(ref normalOnSurfaceB, ref pOnB, distance);
}
}
public void ComputeClosestPoints(ref IndexedMatrix transA, ref IndexedMatrix transB, PointCollector pointCollector)
{
if (m_convexB1 != null)
{
m_simplexSolver.Reset();
GjkPairDetector gjk = new GjkPairDetector(m_convexA, m_convexB1, m_convexA.GetShapeType(), m_convexB1.GetShapeType(), m_convexA.GetMargin(), m_convexB1.GetMargin(), m_simplexSolver, m_penetrationDepthSolver);
ClosestPointInput input = ClosestPointInput.Default();
input.m_transformA = transA;
input.m_transformB = transB;
gjk.GetClosestPoints(ref input, pointCollector, null);
}
else
{
//convex versus plane
ConvexShape convexShape = m_convexA;
StaticPlaneShape planeShape = m_planeShape;
bool hasCollision = false;
IndexedVector3 planeNormal = planeShape.GetPlaneNormal();
float planeConstant = planeShape.GetPlaneConstant();
IndexedMatrix convexWorldTransform = transA;
IndexedMatrix convexInPlaneTrans = transB.Inverse() * convexWorldTransform;
IndexedMatrix planeInConvex = convexWorldTransform.Inverse() * transB;
IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);
IndexedVector3 vtxInPlane = convexInPlaneTrans * vtx;
float distance = IndexedVector3.Dot(planeNormal, vtxInPlane) - planeConstant;
IndexedVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
IndexedVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected;
IndexedVector3 normalOnSurfaceB = transB._basis * planeNormal;
pointCollector.AddContactPoint(
ref normalOnSurfaceB,
ref vtxInPlaneWorld,
distance);
}
}
public static void GetPlaneEquation(StaticPlaneShape plane,out IndexedVector4 equation)
{
equation = new IndexedVector4(plane.GetPlaneNormal(),plane.GetPlaneConstant());
}
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;
StaticPlaneShape planeShape = planeObj.GetCollisionShape() as StaticPlaneShape;
bool hasCollision = false;
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();
}
}
}
public static void GetPlaneEquation(StaticPlaneShape plane, out IndexedVector4 equation)
{
equation = new IndexedVector4(plane.GetPlaneNormal(), plane.GetPlaneConstant());
}