protected void GImpactTrimeshpartVsPlaneCollision(
CollisionObject body0,
CollisionObject body1,
GImpactMeshShapePart shape0,
StaticPlaneShape shape1, bool swapped)
{
IndexedMatrix orgtrans0 = body0.GetWorldTransform();
IndexedMatrix orgtrans1 = body1.GetWorldTransform();
IndexedVector4 plane;
PlaneShape.GetPlaneEquationTransformed(shape1, ref orgtrans1, out plane);
//test box against plane
AABB tribox = new AABB();
shape0.GetAabb(ref orgtrans0, out tribox.m_min, out tribox.m_max);
tribox.IncrementMargin(shape1.GetMargin());
if (tribox.PlaneClassify(ref plane) != BT_PLANE_INTERSECTION_TYPE.BT_CONST_COLLIDE_PLANE)
{
return;
}
shape0.LockChildShapes();
float margin = shape0.GetMargin() + shape1.GetMargin();
IndexedVector3 vertex;
int vi = shape0.GetVertexCount();
while (vi-- != 0)
{
shape0.GetVertex(vi, out vertex);
vertex = orgtrans0 * vertex;
float distance = IndexedVector3.Dot(vertex, MathUtil.Vector4ToVector3(ref plane)) - plane.W - margin;
if (distance < 0.0f)//add contact
{
if (swapped)
{
AddContactPoint(body1, body0,
vertex,
MathUtil.Vector4ToVector3(-plane),
distance);
}
else
{
AddContactPoint(body0, body1,
vertex,
MathUtil.Vector4ToVector3(ref plane),
distance);
}
}
}
shape0.UnlockChildShapes();
}
protected void GImpactTrimeshpartVsPlaneCollision(
CollisionObject body0,
CollisionObject body1,
GImpactMeshShapePart shape0,
StaticPlaneShape shape1, bool swapped)
{
IndexedMatrix orgtrans0 = body0.GetWorldTransform();
IndexedMatrix orgtrans1 = body1.GetWorldTransform();
IndexedVector4 plane;
PlaneShape.GetPlaneEquationTransformed(shape1,ref orgtrans1, out plane);
//test box against plane
AABB tribox = new AABB();
shape0.GetAabb(ref orgtrans0, out tribox.m_min, out tribox.m_max);
tribox.IncrementMargin(shape1.GetMargin());
if (tribox.PlaneClassify(ref plane) != BT_PLANE_INTERSECTION_TYPE.BT_CONST_COLLIDE_PLANE) return;
shape0.LockChildShapes();
float margin = shape0.GetMargin() + shape1.GetMargin();
IndexedVector3 vertex;
int vi = shape0.GetVertexCount();
while (vi-- != 0)
{
shape0.GetVertex(vi, out vertex);
vertex = orgtrans0 * vertex;
float distance = IndexedVector3.Dot(vertex, MathUtil.Vector4ToVector3(ref plane)) - plane.W - margin;
if (distance < 0.0f)//add contact
{
if (swapped)
{
AddContactPoint(body1, body0,
vertex,
MathUtil.Vector4ToVector3(-plane),
distance);
}
else
{
AddContactPoint(body0, body1,
vertex,
MathUtil.Vector4ToVector3(ref plane),
distance);
}
}
}
shape0.UnlockChildShapes();
}
