public bool FindTriangleCollisionClipMethod(PrimitiveTriangle other, GIM_TRIANGLE_CONTACT contacts)
{
float margin = m_margin + other.m_margin;
int clipped_count;
//create planes
// plane v vs U points
m_contacts1.m_separating_normal = m_plane;
clipped_count = ClipTriangle(other, clipped_points);
if (clipped_count == 0)
{
return(false);//Reject
}
//find most deep interval face1
m_contacts1.MergePoints(ref m_contacts1.m_separating_normal, margin, clipped_points, clipped_count);
if (m_contacts1.m_point_count == 0)
{
return(false); // too far
}
//Normal pointing to this triangle
m_contacts1.m_separating_normal *= -1.0f;
//Clip tri1 by tri2 edges
GIM_TRIANGLE_CONTACT contacts2 = new GIM_TRIANGLE_CONTACT();
contacts2.m_separating_normal = other.m_plane;
clipped_count = other.ClipTriangle(this, clipped_points);
if (clipped_count == 0)
{
return(false);//Reject
}
//find most deep interval face1
contacts2.MergePoints(ref contacts2.m_separating_normal, margin, clipped_points, clipped_count);
if (contacts2.m_point_count == 0)
{
return(false); // too far
}
////check most dir for contacts
if (contacts2.m_penetration_depth < m_contacts1.m_penetration_depth)
{
contacts.CopyFrom(contacts2);
}
else
{
contacts.CopyFrom(m_contacts1);
}
return(true);
}
public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
LockChildShapes();
AABB box = new AABB();
box.m_min = aabbMin;
box.m_max = aabbMax;
ObjectArray <int> collided = new ObjectArray <int>();
m_box_set.BoxQuery(ref box, collided);
if (collided.Count == 0)
{
UnlockChildShapes();
return;
}
int part = GetPart();
PrimitiveTriangle triangle = new PrimitiveTriangle();
int i = collided.Count;
while (i-- != 0)
{
GetPrimitiveTriangle(collided[i], triangle);
callback.ProcessTriangle(triangle.m_vertices, part, collided[i]);
}
UnlockChildShapes();
}
//! Test if triangles could collide
public bool OverlapTestConservative(PrimitiveTriangle other)
{
float total_margin = m_margin + other.m_margin;
// classify points on other triangle
float dis0 = ClipPolygon.DistancePointPlane(ref m_plane, ref other.m_vertices[0]) - total_margin;
float dis1 = ClipPolygon.DistancePointPlane(ref m_plane, ref other.m_vertices[1]) - total_margin;
float dis2 = ClipPolygon.DistancePointPlane(ref m_plane, ref other.m_vertices[2]) - total_margin;
if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f)
{
return(false);
}
// classify points on this triangle
dis0 = ClipPolygon.DistancePointPlane(ref other.m_plane, ref m_vertices[0]) - total_margin;
dis1 = ClipPolygon.DistancePointPlane(ref other.m_plane, ref m_vertices[1]) - total_margin;
dis2 = ClipPolygon.DistancePointPlane(ref other.m_plane, ref m_vertices[2]) - total_margin;
if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f)
{
return(false);
}
return(true);
}
public virtual void GetPrimitiveTriangle(int prim_index, PrimitiveTriangle triangle)
{
GetIndices(prim_index, out indicesHolder[0], out indicesHolder[1], out indicesHolder[2]);
GetVertex(indicesHolder[0], out triangle.m_vertices[0]);
GetVertex(indicesHolder[1], out triangle.m_vertices[1]);
GetVertex(indicesHolder[2], out triangle.m_vertices[2]);
triangle.m_margin = m_margin;
}
public virtual void GetPrimitiveBox(int prim_index, out AABB primbox)
{
PrimitiveTriangle triangle = new PrimitiveTriangle();
GetPrimitiveTriangle(prim_index, triangle);
primbox = new AABB();
primbox.CalcFromTriangleMargin(
ref triangle.m_vertices[0],
ref triangle.m_vertices[1], ref triangle.m_vertices[2], triangle.m_margin);
}
public int ClipTriangle(PrimitiveTriangle other, IndexedVector3[] clipped_points)
{
// edge 0
//ObjectArray<IndexedVector3> temp_points = new ObjectArray<IndexedVector3>(GIM_TRIANGLE_CONTACT.MAX_TRI_CLIPPING);
IndexedVector4 edgeplane;
GetEdgePlane(0, out edgeplane);
int clipped_count = ClipPolygon.PlaneClipTriangle(ref edgeplane, ref other.m_vertices[0], ref other.m_vertices[1], ref other.m_vertices[2], temp_points);
if (clipped_count == 0)
{
return(0);
}
//ObjectArray<IndexedVector3> temp_points1 = new ObjectArray<IndexedVector3>(GIM_TRIANGLE_CONTACT.MAX_TRI_CLIPPING);
// edge 1
GetEdgePlane(1, out edgeplane);
clipped_count = ClipPolygon.PlaneClipPolygon(ref edgeplane, temp_points, clipped_count, temp_points1);
if (clipped_count == 0)
{
return(0);
}
// edge 2
GetEdgePlane(2, out edgeplane);
Debug.Assert(clipped_count < GIM_TRIANGLE_CONTACT.MAX_TRI_CLIPPING);
clipped_count = ClipPolygon.PlaneClipPolygon(ref edgeplane, temp_points1, clipped_count, clipped_points);
return(clipped_count);
}
public virtual void GetPrimitiveTriangle(int prim_index, PrimitiveTriangle triangle)
{
GetIndices(prim_index, out indicesHolder[0], out indicesHolder[1], out indicesHolder[2]);
GetVertex(indicesHolder[0], out triangle.m_vertices[0]);
GetVertex(indicesHolder[1], out triangle.m_vertices[1]);
GetVertex(indicesHolder[2], out triangle.m_vertices[2]);
triangle.m_margin = m_margin;
}
public virtual void GetPrimitiveTriangle(int prim_index, PrimitiveTriangle triangle)
{
Debug.Assert(false);
}
//! if this trimesh
public void GetPrimitiveTriangle(int index, PrimitiveTriangle triangle)
{
GetPrimitiveManager().GetPrimitiveTriangle(index, triangle);
}
public void GetNodeTriangle(int nodeindex, PrimitiveTriangle triangle)
{
m_primitive_manager.GetPrimitiveTriangle(GetNodeData(nodeindex), triangle);
}
protected void CollideSatTriangles(CollisionObject body0,
CollisionObject body1,
GImpactMeshShapePart shape0,
GImpactMeshShapePart shape1,
PairSet pairs, int pair_count)
{
IndexedMatrix orgtrans0 = body0.GetWorldTransform();
IndexedMatrix orgtrans1 = body1.GetWorldTransform();
PrimitiveTriangle ptri0 = new PrimitiveTriangle();
PrimitiveTriangle ptri1 = new PrimitiveTriangle();
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::CollideSatTriangles [{0}]", pair_count);
}
shape0.LockChildShapes();
shape1.LockChildShapes();
int pair_pointer = 0;
while (pair_count-- != 0)
{
m_triface0 = pairs[pair_pointer].m_index1;
m_triface1 = pairs[pair_pointer].m_index2;
pair_pointer += 1;
shape0.GetPrimitiveTriangle(m_triface0, ptri0);
shape1.GetPrimitiveTriangle(m_triface1, ptri1);
#if TRI_COLLISION_PROFILING
BulletGlobal.StartProfile("gim02_tri_time");
#endif
ptri0.ApplyTransform(ref orgtrans0);
ptri1.ApplyTransform(ref orgtrans1);
//build planes
ptri0.BuildTriPlane();
ptri1.BuildTriPlane();
// test conservative
if (ptri0.OverlapTestConservative(ptri1))
{
if (ptri0.FindTriangleCollisionClipMethod(ptri1, m_contact_data))
{
int j = m_contact_data.m_point_count;
while (j-- != 0)
{
AddContactPoint(body0, body1,
m_contact_data.m_points[j],
MathUtil.Vector4ToVector3(ref m_contact_data.m_separating_normal),
-m_contact_data.m_penetration_depth);
}
}
}
#if TRI_COLLISION_PROFILING
BulletGlobals.StopProfile();
#endif
}
shape0.UnlockChildShapes();
shape1.UnlockChildShapes();
}
public virtual void GetPrimitiveBox(int prim_index, out AABB primbox)
{
PrimitiveTriangle triangle = new PrimitiveTriangle();
GetPrimitiveTriangle(prim_index, triangle);
primbox = new AABB();
primbox.CalcFromTriangleMargin(
ref triangle.m_vertices[0],
ref triangle.m_vertices[1], ref triangle.m_vertices[2], triangle.m_margin);
}
protected void CollideSatTriangles(CollisionObject body0,
CollisionObject body1,
GImpactMeshShapePart shape0,
GImpactMeshShapePart shape1,
PairSet pairs, int pair_count)
{
IndexedMatrix orgtrans0 = body0.GetWorldTransform();
IndexedMatrix orgtrans1 = body1.GetWorldTransform();
PrimitiveTriangle ptri0 = new PrimitiveTriangle();
PrimitiveTriangle ptri1 = new PrimitiveTriangle();
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::CollideSatTriangles [{0}]", pair_count);
}
shape0.LockChildShapes();
shape1.LockChildShapes();
int pair_pointer = 0;
while (pair_count-- != 0)
{
m_triface0 = pairs[pair_pointer].m_index1;
m_triface1 = pairs[pair_pointer].m_index2;
pair_pointer += 1;
shape0.GetPrimitiveTriangle(m_triface0, ptri0);
shape1.GetPrimitiveTriangle(m_triface1, ptri1);
#if TRI_COLLISION_PROFILING
BulletGlobal.StartProfile("gim02_tri_time");
#endif
ptri0.ApplyTransform(ref orgtrans0);
ptri1.ApplyTransform(ref orgtrans1);
//build planes
ptri0.BuildTriPlane();
ptri1.BuildTriPlane();
// test conservative
if (ptri0.OverlapTestConservative(ptri1))
{
if (ptri0.FindTriangleCollisionClipMethod(ptri1, m_contact_data))
{
int j = m_contact_data.m_point_count;
while (j-- != 0)
{
AddContactPoint(body0, body1,
m_contact_data.m_points[j],
MathUtil.Vector4ToVector3(ref m_contact_data.m_separating_normal),
-m_contact_data.m_penetration_depth);
}
}
}
#if TRI_COLLISION_PROFILING
BulletGlobals.StopProfile();
#endif
}
shape0.UnlockChildShapes();
shape1.UnlockChildShapes();
}
public virtual void GetPrimitiveTriangle(int prim_index, PrimitiveTriangle triangle)
{
Debug.Assert(false);
}
//! if this trimesh
public void GetPrimitiveTriangle(int index, PrimitiveTriangle triangle)
{
GetPrimitiveManager().GetPrimitiveTriangle(index, triangle);
}
public bool FindTriangleCollisionClipMethod(PrimitiveTriangle other, GIM_TRIANGLE_CONTACT contacts)
{
float margin = m_margin + other.m_margin;
int clipped_count;
//create planes
// plane v vs U points
m_contacts1.m_separating_normal = m_plane;
clipped_count = ClipTriangle(other, clipped_points);
if (clipped_count == 0)
{
return false;//Reject
}
//find most deep interval face1
m_contacts1.MergePoints(ref m_contacts1.m_separating_normal, margin, clipped_points, clipped_count);
if (m_contacts1.m_point_count == 0)
{
return false; // too far
}
//Normal pointing to this triangle
m_contacts1.m_separating_normal *= -1.0f;
//Clip tri1 by tri2 edges
GIM_TRIANGLE_CONTACT contacts2 = new GIM_TRIANGLE_CONTACT();
contacts2.m_separating_normal = other.m_plane;
clipped_count = other.ClipTriangle(this, clipped_points);
if (clipped_count == 0)
{
return false;//Reject
}
//find most deep interval face1
contacts2.MergePoints(ref contacts2.m_separating_normal, margin, clipped_points, clipped_count);
if (contacts2.m_point_count == 0)
{
return false; // too far
}
////check most dir for contacts
if (contacts2.m_penetration_depth < m_contacts1.m_penetration_depth)
{
contacts.CopyFrom(contacts2);
}
else
{
contacts.CopyFrom(m_contacts1);
}
return true;
}
public int ClipTriangle(PrimitiveTriangle other, IndexedVector3[] clipped_points)
{
// edge 0
//ObjectArray<IndexedVector3> temp_points = new ObjectArray<IndexedVector3>(GIM_TRIANGLE_CONTACT.MAX_TRI_CLIPPING);
IndexedVector4 edgeplane;
GetEdgePlane(0, out edgeplane);
int clipped_count = ClipPolygon.PlaneClipTriangle(ref edgeplane, ref other.m_vertices[0], ref other.m_vertices[1], ref other.m_vertices[2], temp_points);
if (clipped_count == 0)
{
return 0;
}
//ObjectArray<IndexedVector3> temp_points1 = new ObjectArray<IndexedVector3>(GIM_TRIANGLE_CONTACT.MAX_TRI_CLIPPING);
// edge 1
GetEdgePlane(1, out edgeplane);
clipped_count = ClipPolygon.PlaneClipPolygon(ref edgeplane, temp_points, clipped_count, temp_points1);
if (clipped_count == 0)
{
return 0;
}
// edge 2
GetEdgePlane(2, out edgeplane);
Debug.Assert(clipped_count < GIM_TRIANGLE_CONTACT.MAX_TRI_CLIPPING);
clipped_count = ClipPolygon.PlaneClipPolygon(ref edgeplane, temp_points1, clipped_count, clipped_points);
return clipped_count;
}
//! Test if triangles could collide
public bool OverlapTestConservative(PrimitiveTriangle other)
{
float total_margin = m_margin + other.m_margin;
// classify points on other triangle
float dis0 = ClipPolygon.DistancePointPlane(ref m_plane, ref other.m_vertices[0]) - total_margin;
float dis1 = ClipPolygon.DistancePointPlane(ref m_plane, ref other.m_vertices[1]) - total_margin;
float dis2 = ClipPolygon.DistancePointPlane(ref m_plane, ref other.m_vertices[2]) - total_margin;
if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false;
// classify points on this triangle
dis0 = ClipPolygon.DistancePointPlane(ref other.m_plane, ref m_vertices[0]) - total_margin;
dis1 = ClipPolygon.DistancePointPlane(ref other.m_plane, ref m_vertices[1]) - total_margin;
dis2 = ClipPolygon.DistancePointPlane(ref other.m_plane, ref m_vertices[2]) - total_margin;
if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false;
return true;
}
void IPrimitiveManagerBase.GetPrimitiveTriangle(int prim_index, PrimitiveTriangle triangle)
{
triangle.m_vertices[0] = GetVertex(Triangles[prim_index].I0).ToBullet();
triangle.m_vertices[1] = GetVertex(Triangles[prim_index].I1).ToBullet();
triangle.m_vertices[2] = GetVertex(Triangles[prim_index].I2).ToBullet();
}
public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
LockChildShapes();
AABB box = new AABB();
box.m_min = aabbMin;
box.m_max = aabbMax;
ObjectArray<int> collided = new ObjectArray<int>();
m_box_set.BoxQuery(ref box, collided);
if (collided.Count == 0)
{
UnlockChildShapes();
return;
}
int part = GetPart();
PrimitiveTriangle triangle = new PrimitiveTriangle();
int i = collided.Count;
while (i-- != 0)
{
GetPrimitiveTriangle(collided[i], triangle);
callback.ProcessTriangle(triangle.m_vertices, part, collided[i]);
}
UnlockChildShapes();
}
