public override void InitializeDemo()
{
SetCameraDistance(50);
int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1);
int vertStride = 1;
int indexStride = 3;
BulletGlobals.gContactAddedCallback = new CustomMaterialCombinerCallback();
gVertices = new ObjectArray<IndexedVector3>(totalVerts);
gIndices = new ObjectArray<int>(totalTriangles * 3);
SetVertexPositions(waveheight, 0.0f);
gVertices.GetRawArray()[1].Y = 0.1f;
int index=0;
int i, j;
for (i=0;i<NUM_VERTS_X-1;i++)
{
for (j=0;j<NUM_VERTS_Y-1;j++)
{
#if SWAP_WINDING
#if SHIFT_INDICES
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
#else
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
#endif //SHIFT_INDICES
#else //SWAP_WINDING
#if SHIFT_INDICES
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = j*NUM_VERTS_X+i+1;
#if TEST_INCONSISTENT_WINDING
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
#else //TEST_INCONSISTENT_WINDING
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
#endif //TEST_INCONSISTENT_WINDING
#else //SHIFT_INDICES
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = j*NUM_VERTS_X+i+1;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
#endif //SHIFT_INDICES
#endif //SWAP_WINDING
}
}
m_indexVertexArrays = new TriangleIndexVertexArray(totalTriangles,
gIndices,
indexStride,
totalVerts, gVertices, vertStride);
bool useQuantizedAabbCompression = true;
IndexedVector3 aabbMin = new IndexedVector3 (-1000,-1000,-1000);
IndexedVector3 aabbMax = new IndexedVector3(1000, 1000, 1000);
trimeshShape = new BvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,ref aabbMin,ref aabbMax,true);
CollisionShape groundShape = trimeshShape;
TriangleInfoMap triangleInfoMap = new TriangleInfoMap();
InternalEdgeUtility.GenerateInternalEdgeInfo(trimeshShape, triangleInfoMap);
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
m_broadphase = new DbvtBroadphase();
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
m_dynamicsWorld.SetDebugDrawer(m_debugDraw);
IndexedVector3 gravity = new IndexedVector3(0,-10,0);
m_dynamicsWorld.SetGravity(ref gravity);
float mass = 0.0f;
IndexedMatrix startTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0,-2,0));
ConvexHullShape colShape = new ConvexHullShape(new List<IndexedVector3>(), 0);
for (int k=0;k<DemoMeshes.TaruVtxCount;k++)
{
IndexedVector3 vtx = DemoMeshes.TaruVtx[k];
colShape.AddPoint(ref vtx);
}
//this will enable polyhedral contact clipping, better quality, slightly slower
//colShape.InitializePolyhedralFeatures();
//the polyhedral contact clipping can use either GJK or SAT test to find the separating axis
m_dynamicsWorld.GetDispatchInfo().m_enableSatConvex=false;
{
//for (int i2 = 0; i2 < 1; i2++)
//{
// startTransform._origin = new IndexedVector3(-10.0f + i2 * 3.0f, 2.2f + i2 * 0.1f, -1.3f);
// RigidBody body = LocalCreateRigidBody(10, startTransform, colShape);
// body.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// body.SetLinearVelocity(new IndexedVector3(0, 0, -1));
// //body->setContactProcessingThreshold(0.f);
//}
}
{
BoxShape colShape2 = new BoxShape(new IndexedVector3(1, 1, 1));
//colShape.InitializePolyhedralFeatures();
m_collisionShapes.Add(colShape2);
startTransform._origin = new IndexedVector3(-16.0f + i * 3.0f, 1.0f + i * 0.1f, -1.3f);
RigidBody body = LocalCreateRigidBody(10, startTransform, colShape2);
body.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
body.SetLinearVelocity(new IndexedVector3(0, 0, -1));
}
startTransform = IndexedMatrix.Identity;
staticBody = LocalCreateRigidBody(mass, startTransform,groundShape);
//staticBody->setContactProcessingThreshold(-0.031f);
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_KINEMATIC_OBJECT);//STATIC_OBJECT);
//enable custom material callback
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK);
m_debugDraw.SetDebugMode(DebugDrawModes.DBG_DrawText | DebugDrawModes.DBG_NoHelpText | DebugDrawModes.DBG_DrawWireframe | DebugDrawModes.DBG_DrawContactPoints);
//base.InitializeDemo();
//ClientResetScene();
}
private static void ProcessResult(IndexedVector3[] triangleVerts, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax, BvhTriangleMeshShape trimeshShape, int partId, int triangleIndex, TriangleInfoMap triangleInfoMap)
{
aabbMin = MathUtil.MAX_VECTOR;
aabbMax = MathUtil.MIN_VECTOR;
aabbMin.SetMin(ref triangleVerts[0]);
aabbMax.SetMax(ref triangleVerts[0]);
aabbMin.SetMin(ref triangleVerts[1]);
aabbMax.SetMax(ref triangleVerts[1]);
aabbMin.SetMin(ref triangleVerts[2]);
aabbMax.SetMax(ref triangleVerts[2]);
ConnectivityProcessor connectivityProcessor = new ConnectivityProcessor();
connectivityProcessor.m_partIdA = partId;
connectivityProcessor.m_triangleIndexA = triangleIndex;
connectivityProcessor.m_triangleVerticesA = triangleVerts;
connectivityProcessor.m_triangleInfoMap = triangleInfoMap;
trimeshShape.ProcessAllTriangles(connectivityProcessor, ref aabbMin, ref aabbMax);
}
/// Changes a btManifoldPoint collision normal to the normal from the mesh.
public static void AdjustInternalEdgeContacts(ManifoldPoint cp, CollisionObject colObj0, CollisionObject colObj1, int partId0, int index0, InternalEdgeAdjustFlags normalAdjustFlags)
{
//btAssert(colObj0.GetCollisionShape().GetShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
if (colObj0.GetCollisionShape().GetShapeType() != BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
{
return;
}
BvhTriangleMeshShape trimesh = null;
if (colObj0.GetRootCollisionShape().GetShapeType() == BroadphaseNativeTypes.SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
//trimesh = ((ScaledBvhTriangleMeshShape)colObj0.GetRootCollisionShape()).GetChildShape();
}
else
{
trimesh = (BvhTriangleMeshShape)colObj0.GetRootCollisionShape();
}
TriangleInfoMap triangleInfoMapPtr = (TriangleInfoMap)trimesh.GetTriangleInfoMap();
if (triangleInfoMapPtr == null)
{
return;
}
int hash = GetHash(partId0, index0);
TriangleInfo info;
if (!triangleInfoMapPtr.TryGetValue(hash, out info))
{
return;
}
float frontFacing = (normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_BACKFACE_MODE) == 0 ? 1.0f : -1.0f;
TriangleShape tri_shape = colObj0.GetCollisionShape() as TriangleShape;
IndexedVector3 v0, v1, v2;
tri_shape.GetVertex(0, out v0);
tri_shape.GetVertex(1, out v1);
tri_shape.GetVertex(2, out v2);
IndexedVector3 center = (v0 + v1 + v2) * (1.0f / 3.0f);
IndexedVector3 red = new IndexedVector3(1, 0, 0), green = new IndexedVector3(0, 1, 0), blue = new IndexedVector3(0, 0, 1), white = new IndexedVector3(1, 1, 1), black = new IndexedVector3(0, 0, 0);
IndexedVector3 tri_normal;
tri_shape.CalcNormal(out tri_normal);
//float dot = tri_normal.dot(cp.m_normalWorldOnB);
IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest);
IndexedVector3 contact = cp.m_localPointB;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedMatrix tr = colObj0.GetWorldTransform();
DebugDrawLine(tr * nearest, tr * cp.m_localPointB, red);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
bool isNearEdge = false;
int numConcaveEdgeHits = 0;
int numConvexEdgeHits = 0;
IndexedVector3 localContactNormalOnB = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
localContactNormalOnB.Normalize();//is this necessary?
// Get closest edge
int bestedge = -1;
float disttobestedge = MathUtil.BT_LARGE_FLOAT;
//
// Edge 0 . 1
if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
//IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest);
float len = (contact - nearest).Length();
//
if (len < disttobestedge)
{
bestedge = 0;
disttobestedge = len;
}
}
// Edge 1 . 2
if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
//IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v1, ref v2, out nearest);
float len = (contact - nearest).Length();
//
if (len < disttobestedge)
{
bestedge = 1;
disttobestedge = len;
}
}
// Edge 2 . 0
if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
//IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v2, ref v0, out nearest);
float len = (contact - nearest).Length();
//
if (len < disttobestedge)
{
bestedge = 2;
disttobestedge = len;
}
}
#if BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 upfix = tri_normal * new IndexedVector3(0.1f, 0.1f, 0.1f);
DebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red);
#endif
if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif
float len = (contact - nearest).Length();
if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
{
if (bestedge == 0)
{
IndexedVector3 edge = (v0 - v1);
isNearEdge = true;
if (info.m_edgeV0V1Angle == 0.0f)
{
numConcaveEdgeHits++;
}
else
{
bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_CONVEX) != 0;
float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 nA = swapFactor * tri_normal;
IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV0V1Angle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
if ((info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB) != 0)
{
computedNormalB *= -1;
}
IndexedVector3 nB = swapFactor * computedNormalB;
float NdotA = localContactNormalOnB.Dot(ref nA);
float NdotB = localContactNormalOnB.Dot(ref nB);
bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);
#if DEBUG_INTERNAL_EDGE
{
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
}
#endif //DEBUG_INTERNAL_EDGE
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
IndexedVector3 clampedLocalNormal;
bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info.m_edgeV0V1Angle, out clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
{
IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis *clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal. (what about cp.m_distance1?)
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
}
NearestPointInLineSegment(ref contact, ref v1, ref v2, out nearest);
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * cp.m_localPointB, green);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green);
#endif
if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
float len = (contact - nearest).Length();
if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
{
if (bestedge == 1)
{
isNearEdge = true;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 edge = (v1 - v2);
isNearEdge = true;
if (info.m_edgeV1V2Angle == 0f)
{
numConcaveEdgeHits++;
}
else
{
bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_CONVEX) != 0;
float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 nA = swapFactor * tri_normal;
IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV1V2Angle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
if ((info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_SWAP_NORMALB) != 0)
{
computedNormalB *= -1;
}
IndexedVector3 nB = swapFactor * computedNormalB;
#if DEBUG_INTERNAL_EDGE
{
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
}
#endif //DEBUG_INTERNAL_EDGE
float NdotA = localContactNormalOnB.Dot(ref nA);
float NdotB = localContactNormalOnB.Dot(ref nB);
bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
IndexedVector3 clampedLocalNormal;
bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV1V2Angle, out clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
{
IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis *clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
}
NearestPointInLineSegment(ref contact, ref v2, ref v0, out nearest);
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * cp.m_localPointB, blue);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue);
#endif
if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
float len = (contact - nearest).Length();
if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
{
if (bestedge == 2)
{
isNearEdge = true;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 edge = (v2 - v0);
if (info.m_edgeV2V0Angle == 0f)
{
numConcaveEdgeHits++;
}
else
{
bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_CONVEX) != 0;
float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 nA = swapFactor * tri_normal;
IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV2V0Angle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
if ((info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_SWAP_NORMALB) != 0)
{
computedNormalB *= -1;
}
IndexedVector3 nB = swapFactor * computedNormalB;
#if DEBUG_INTERNAL_EDGE
{
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
}
#endif //DEBUG_INTERNAL_EDGE
float NdotA = localContactNormalOnB.Dot(ref nA);
float NdotB = localContactNormalOnB.Dot(ref nB);
bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
// printf("hitting convex edge\n");
IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
IndexedVector3 clampedLocalNormal;
bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV2V0Angle, out clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
{
IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis *clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
}
#if DEBUG_INTERNAL_EDGE
{
IndexedVector3 color = new IndexedVector3(0, 1, 1);
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + cp.m_normalWorldOnB * 10, color);
}
#endif //DEBUG_INTERNAL_EDGE
if (isNearEdge)
{
if (numConcaveEdgeHits > 0)
{
if ((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONCAVE_DOUBLE_SIDED) != 0)
{
//fix tri_normal so it pointing the same direction as the current local contact normal
if (tri_normal.Dot(ref localContactNormalOnB) < 0)
{
tri_normal *= -1;
}
cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis *tri_normal;
}
else
{
IndexedVector3 newNormal = tri_normal * frontFacing;
//if the tri_normal is pointing opposite direction as the current local contact normal, skip it
float d = newNormal.Dot(ref localContactNormalOnB);
if (d < 0)
{
return;
}
//modify the normal to be the triangle normal (or backfacing normal)
cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis *newNormal;
}
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
/////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////
public static void GenerateInternalEdgeInfo(BvhTriangleMeshShape trimeshShape, TriangleInfoMap triangleInfoMap)
{
//the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
if (trimeshShape.GetTriangleInfoMap() != null)
{
return;
}
trimeshShape.SetTriangleInfoMap(triangleInfoMap);
StridingMeshInterface meshInterface = trimeshShape.GetMeshInterface();
IndexedVector3 meshScaling = meshInterface.GetScaling();
for (int partId = 0; partId < meshInterface.GetNumSubParts(); partId++)
{
object vertexbase = null;
int numverts = 0;
PHY_ScalarType type = PHY_ScalarType.PHY_INTEGER;
int stride = 0;
object indexbase = null;
int indexstride = 0;
int numfaces = 0;
PHY_ScalarType indicestype = PHY_ScalarType.PHY_INTEGER;
//PHY_ScalarType indexType=0;
IndexedVector3[] triangleVerts = new IndexedVector3[3];
meshInterface.GetLockedReadOnlyVertexIndexBase(out vertexbase, out numverts, out type, out stride, out indexbase, out indexstride, out numfaces, out indicestype, partId);
IndexedVector3 aabbMin, aabbMax;
switch (indicestype)
{
case PHY_ScalarType.PHY_INTEGER:
{
int[] indexList = ((ObjectArray <int>)indexbase).GetRawArray();
if (vertexbase is ObjectArray <IndexedVector3> )
{
IndexedVector3[] vertexList = (vertexbase as ObjectArray <IndexedVector3>).GetRawArray();
int indexCounter = 0;
for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++)
{
int index1 = indexList[triangleIndex];
int index2 = indexList[triangleIndex + 1];
int index3 = indexList[triangleIndex + 2];
triangleVerts[0] = new IndexedVector3(vertexList[index1]) * meshScaling;
triangleVerts[1] = new IndexedVector3(vertexList[index2]) * meshScaling;
triangleVerts[2] = new IndexedVector3(vertexList[index3]) * meshScaling;
ProcessResult(triangleVerts, out aabbMin, out aabbMax, trimeshShape, partId, triangleIndex, triangleInfoMap);
}
}
#if XNA
else if (vertexbase is ObjectArray <Microsoft.Xna.Framework.Vector3> )
{
Microsoft.Xna.Framework.Vector3[] vertexList = (vertexbase as ObjectArray <Microsoft.Xna.Framework.Vector3>).GetRawArray();
int indexCounter = 0;
for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++)
{
int index1 = indexList[triangleIndex];
int index2 = indexList[triangleIndex + 1];
int index3 = indexList[triangleIndex + 2];
triangleVerts[0] = new IndexedVector3(vertexList[index1]) * meshScaling;
triangleVerts[1] = new IndexedVector3(vertexList[index2]) * meshScaling;
triangleVerts[2] = new IndexedVector3(vertexList[index3]) * meshScaling;
ProcessResult(triangleVerts, out aabbMin, out aabbMax, trimeshShape, partId, triangleIndex, triangleInfoMap);
}
}
#endif
else if (vertexbase is ObjectArray <float> )
{
float[] vertexList = (vertexbase as ObjectArray <float>).GetRawArray();
for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++)
{
triangleVerts[0] = new IndexedVector3(vertexList[indexList[triangleIndex]], vertexList[indexList[triangleIndex] + 1], vertexList[indexList[triangleIndex] + 2]) * meshScaling;
triangleVerts[1] = new IndexedVector3(vertexList[indexList[triangleIndex + 1]], vertexList[indexList[triangleIndex + 1] + 1], vertexList[indexList[triangleIndex + 1] + 2]) * meshScaling;
triangleVerts[2] = new IndexedVector3(vertexList[indexList[triangleIndex + 2]], vertexList[indexList[triangleIndex + 2] + 1], vertexList[indexList[triangleIndex + 2] + 2]) * meshScaling;
ProcessResult(triangleVerts, out aabbMin, out aabbMax, trimeshShape, partId, triangleIndex, triangleInfoMap);
}
}
break;
}
default:
{
Debug.Assert(indicestype == PHY_ScalarType.PHY_INTEGER);
break;
}
}
}
}
private static void ProcessResult(IndexedVector3[] triangleVerts, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax, BvhTriangleMeshShape trimeshShape, int partId, int triangleIndex, TriangleInfoMap triangleInfoMap)
{
aabbMin = MathUtil.MAX_VECTOR;
aabbMax = MathUtil.MIN_VECTOR;
aabbMin.SetMin(ref triangleVerts[0]);
aabbMax.SetMax(ref triangleVerts[0]);
aabbMin.SetMin(ref triangleVerts[1]);
aabbMax.SetMax(ref triangleVerts[1]);
aabbMin.SetMin(ref triangleVerts[2]);
aabbMax.SetMax(ref triangleVerts[2]);
ConnectivityProcessor connectivityProcessor = new ConnectivityProcessor();
connectivityProcessor.m_partIdA = partId;
connectivityProcessor.m_triangleIndexA = triangleIndex;
connectivityProcessor.m_triangleVerticesA = triangleVerts;
connectivityProcessor.m_triangleInfoMap = triangleInfoMap;
trimeshShape.ProcessAllTriangles(connectivityProcessor, ref aabbMin, ref aabbMax);
}
/////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////
public static void GenerateInternalEdgeInfo(BvhTriangleMeshShape trimeshShape, TriangleInfoMap triangleInfoMap)
{
//the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
if (trimeshShape.GetTriangleInfoMap() != null)
{
return;
}
trimeshShape.SetTriangleInfoMap(triangleInfoMap);
StridingMeshInterface meshInterface = trimeshShape.GetMeshInterface();
IndexedVector3 meshScaling = meshInterface.GetScaling();
for (int partId = 0; partId < meshInterface.GetNumSubParts(); partId++)
{
object vertexbase = null;
int numverts = 0;
PHY_ScalarType type = PHY_ScalarType.PHY_INTEGER;
int stride = 0;
object indexbase = null;
int indexstride = 0;
int numfaces = 0;
PHY_ScalarType indicestype = PHY_ScalarType.PHY_INTEGER;
//PHY_ScalarType indexType=0;
IndexedVector3[] triangleVerts = new IndexedVector3[3];
meshInterface.GetLockedReadOnlyVertexIndexBase(out vertexbase, out numverts, out type, out stride, out indexbase, out indexstride, out numfaces, out indicestype, partId);
IndexedVector3 aabbMin, aabbMax;
switch (indicestype)
{
case PHY_ScalarType.PHY_INTEGER:
{
int[] indexList = ((ObjectArray<int>)indexbase).GetRawArray();
if (vertexbase is ObjectArray<IndexedVector3>)
{
IndexedVector3[] vertexList = (vertexbase as ObjectArray<IndexedVector3>).GetRawArray();
int indexCounter = 0;
for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++)
{
int index1 = indexList[triangleIndex];
int index2 = indexList[triangleIndex + 1];
int index3 = indexList[triangleIndex + 2];
triangleVerts[0] = new IndexedVector3(vertexList[index1]) * meshScaling;
triangleVerts[1] = new IndexedVector3(vertexList[index2]) * meshScaling;
triangleVerts[2] = new IndexedVector3(vertexList[index3]) * meshScaling;
ProcessResult(triangleVerts, out aabbMin, out aabbMax, trimeshShape, partId, triangleIndex, triangleInfoMap);
}
}
#if XNA
else if (vertexbase is ObjectArray<Microsoft.Xna.Framework.Vector3>)
{
Microsoft.Xna.Framework.Vector3[] vertexList = (vertexbase as ObjectArray<Microsoft.Xna.Framework.Vector3>).GetRawArray();
int indexCounter = 0;
for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++)
{
int index1 = indexList[triangleIndex];
int index2 = indexList[triangleIndex + 1];
int index3 = indexList[triangleIndex + 2];
triangleVerts[0] = new IndexedVector3(vertexList[index1]) * meshScaling;
triangleVerts[1] = new IndexedVector3(vertexList[index2]) * meshScaling;
triangleVerts[2] = new IndexedVector3(vertexList[index3]) * meshScaling;
ProcessResult(triangleVerts, out aabbMin, out aabbMax, trimeshShape, partId, triangleIndex, triangleInfoMap);
}
}
#endif
else if (vertexbase is ObjectArray<float>)
{
float[] vertexList = (vertexbase as ObjectArray<float>).GetRawArray();
for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++)
{
triangleVerts[0] = new IndexedVector3(vertexList[indexList[triangleIndex]], vertexList[indexList[triangleIndex] + 1], vertexList[indexList[triangleIndex] + 2]) * meshScaling;
triangleVerts[1] = new IndexedVector3(vertexList[indexList[triangleIndex + 1]], vertexList[indexList[triangleIndex + 1] + 1], vertexList[indexList[triangleIndex + 1] + 2]) * meshScaling;
triangleVerts[2] = new IndexedVector3(vertexList[indexList[triangleIndex + 2]], vertexList[indexList[triangleIndex + 2] + 1], vertexList[indexList[triangleIndex + 2] + 2]) * meshScaling;
ProcessResult(triangleVerts, out aabbMin, out aabbMax, trimeshShape, partId, triangleIndex, triangleInfoMap);
}
}
break;
}
default:
{
Debug.Assert(indicestype == PHY_ScalarType.PHY_INTEGER);
break;
}
}
}
}
public override void InitializeDemo()
{
SetCameraDistance(50);
int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1);
int vertStride = 1;
int indexStride = 3;
BulletGlobals.gContactAddedCallback = new CustomMaterialCombinerCallback();
gVertices = new ObjectArray <IndexedVector3>(totalVerts);
gIndices = new ObjectArray <int>(totalTriangles * 3);
SetVertexPositions(waveheight, 0.0f);
gVertices.GetRawArray()[1].Y = 0.1f;
int index = 0;
int i, j;
for (i = 0; i < NUM_VERTS_X - 1; i++)
{
for (j = 0; j < NUM_VERTS_Y - 1; j++)
{
#if SWAP_WINDING
#if SHIFT_INDICES
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
#else
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
#endif //SHIFT_INDICES
#else //SWAP_WINDING
#if SHIFT_INDICES
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = j * NUM_VERTS_X + i + 1;
#if TEST_INCONSISTENT_WINDING
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
#else //TEST_INCONSISTENT_WINDING
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
#endif //TEST_INCONSISTENT_WINDING
#else //SHIFT_INDICES
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = j * NUM_VERTS_X + i + 1;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
#endif //SHIFT_INDICES
#endif //SWAP_WINDING
}
}
m_indexVertexArrays = new TriangleIndexVertexArray(totalTriangles,
gIndices,
indexStride,
totalVerts, gVertices, vertStride);
bool useQuantizedAabbCompression = true;
IndexedVector3 aabbMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 aabbMax = new IndexedVector3(1000, 1000, 1000);
trimeshShape = new BvhTriangleMeshShape(m_indexVertexArrays, useQuantizedAabbCompression, ref aabbMin, ref aabbMax, true);
CollisionShape groundShape = trimeshShape;
TriangleInfoMap triangleInfoMap = new TriangleInfoMap();
InternalEdgeUtility.GenerateInternalEdgeInfo(trimeshShape, triangleInfoMap);
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
m_broadphase = new DbvtBroadphase();
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
m_dynamicsWorld.SetDebugDrawer(m_debugDraw);
IndexedVector3 gravity = new IndexedVector3(0, -10, 0);
m_dynamicsWorld.SetGravity(ref gravity);
float mass = 0.0f;
IndexedMatrix startTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, -2, 0));
ConvexHullShape colShape = new ConvexHullShape(new List <IndexedVector3>(), 0);
for (int k = 0; k < DemoMeshes.TaruVtxCount; k++)
{
IndexedVector3 vtx = DemoMeshes.TaruVtx[k];
colShape.AddPoint(ref vtx);
}
//this will enable polyhedral contact clipping, better quality, slightly slower
//colShape.InitializePolyhedralFeatures();
//the polyhedral contact clipping can use either GJK or SAT test to find the separating axis
m_dynamicsWorld.GetDispatchInfo().m_enableSatConvex = false;
{
//for (int i2 = 0; i2 < 1; i2++)
//{
// startTransform._origin = new IndexedVector3(-10.0f + i2 * 3.0f, 2.2f + i2 * 0.1f, -1.3f);
// RigidBody body = LocalCreateRigidBody(10, startTransform, colShape);
// body.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// body.SetLinearVelocity(new IndexedVector3(0, 0, -1));
// //body->setContactProcessingThreshold(0.f);
//}
}
{
BoxShape colShape2 = new BoxShape(new IndexedVector3(1, 1, 1));
//colShape.InitializePolyhedralFeatures();
m_collisionShapes.Add(colShape2);
startTransform._origin = new IndexedVector3(-16.0f + i * 3.0f, 1.0f + i * 0.1f, -1.3f);
RigidBody body = LocalCreateRigidBody(10, startTransform, colShape2);
body.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
body.SetLinearVelocity(new IndexedVector3(0, 0, -1));
}
startTransform = IndexedMatrix.Identity;
staticBody = LocalCreateRigidBody(mass, startTransform, groundShape);
//staticBody->setContactProcessingThreshold(-0.031f);
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_KINEMATIC_OBJECT); //STATIC_OBJECT);
//enable custom material callback
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK);
m_debugDraw.SetDebugMode(DebugDrawModes.DBG_DrawText | DebugDrawModes.DBG_NoHelpText | DebugDrawModes.DBG_DrawWireframe | DebugDrawModes.DBG_DrawContactPoints);
//base.InitializeDemo();
//ClientResetScene();
}
public void SetTriangleInfoMap(TriangleInfoMap triangleInfoMap)
{
m_triangleInfoMap = triangleInfoMap;
}
