public override void SyncPosition()
{
ChCoordsys mcsys = this.mcontactable.GetCsysForCollisionModel();
bt_collision_object.GetWorldTransform()._origin = new IndexedVector3(
(float)mcsys.pos.x, (float)mcsys.pos.y, (float)mcsys.pos.z);
ChMatrix33 <double> rA = new ChMatrix33 <double>(mcsys.rot);
IndexedBasisMatrix basisA = new IndexedBasisMatrix((float)rA.nm.matrix[0, 0], (float)rA.nm.matrix[0, 1], (float)rA.nm.matrix[0, 2], (float)rA.nm.matrix[1, 0],
(float)rA.nm.matrix[1, 1], (float)rA.nm.matrix[1, 2], (float)rA.nm.matrix[2, 0], (float)rA.nm.matrix[2, 1],
(float)rA.nm.matrix[2, 2]);
bt_collision_object.GetWorldTransform()._basis = basisA;
}
public override void Update(GameTime gameTime)
{
if (CollisionObject != null)
{
world = CollisionObject.GetWorldTransform();
position = world.Translation;
rotation = Quaternion.CreateFromRotationMatrix(world);
}
base.Update(gameTime);
}
//response between two dynamic objects without friction, assuming 0 penetration depth
public static float ResolveSingleCollision(
RigidBody body1,
CollisionObject colObj2,
ref IndexedVector3 contactPositionWorld,
ref IndexedVector3 contactNormalOnB,
ContactSolverInfo solverInfo,
float distance)
{
RigidBody body2 = RigidBody.Upcast(colObj2);
IndexedVector3 normal = contactNormalOnB;
IndexedVector3 rel_pos1 = contactPositionWorld - body1.GetWorldTransform()._origin;
IndexedVector3 rel_pos2 = contactPositionWorld - colObj2.GetWorldTransform()._origin;
IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
IndexedVector3 vel2 = body2 != null?body2.GetVelocityInLocalPoint(ref rel_pos2) : IndexedVector3.Zero;
IndexedVector3 vel = vel1 - vel2;
float rel_vel = normal.Dot(ref vel);
float combinedRestitution = body1.GetRestitution() * colObj2.GetRestitution();
float restitution = combinedRestitution * -rel_vel;
float positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep;
float velocityError = -(1.0f + restitution) * rel_vel; // * damping;
float denom0 = body1.ComputeImpulseDenominator(ref contactPositionWorld, ref normal);
float denom1 = body2 != null?body2.ComputeImpulseDenominator(ref contactPositionWorld, ref normal) : 0.0f;
float relaxation = 1.0f;
float jacDiagABInv = relaxation / (denom0 + denom1);
float penetrationImpulse = positionalError * jacDiagABInv;
float velocityImpulse = velocityError * jacDiagABInv;
float normalImpulse = penetrationImpulse + velocityImpulse;
normalImpulse = 0.0f > normalImpulse ? 0.0f : normalImpulse;
body1.ApplyImpulse(normal * (normalImpulse), rel_pos1);
if (body2 != null)
{
body2.ApplyImpulse(-normal * (normalImpulse), rel_pos2);
}
return(normalImpulse);
}
public void InitInstancedRender(AlignedCollisionObjectArray objects)
{
// Clear instance data
foreach (ShapeData s in shapes.Values)
{
s.InstanceDataList.Clear();
}
int i = objects.Count - 1;
for (; i >= 0; i--)
{
CollisionObject colObj = objects[i];
Matrix transform;
if (colObj is SoftBody)
{
if (demo.IsDebugDrawEnabled)
{
continue;
}
transform = Matrix.Identity;
}
else
{
colObj.GetWorldTransform(out transform);
}
InitInstanceData(colObj, colObj.CollisionShape, ref transform);
}
foreach (KeyValuePair <CollisionShape, ShapeData> sh in shapes)
{
ShapeData s = sh.Value;
int instanceCount = s.InstanceDataList.Count;
// Is the instance buffer the right size?
if (s.InstanceDataBuffer.Description.SizeInBytes != instanceCount * InstanceData.SizeInBytes)
{
// No, recreate it
s.InstanceDataBuffer.Dispose();
if (instanceCount == 0)
{
if (s.IndexBuffer != null)
{
s.IndexBuffer.Dispose();
}
s.VertexBuffer.Dispose();
removeList.Add(sh.Key);
continue;
}
instanceDataDesc.SizeInBytes = instanceCount * InstanceData.SizeInBytes;
s.InstanceDataBuffer = new Buffer(device, instanceDataDesc);
s.BufferBindings[1] = new VertexBufferBinding(s.InstanceDataBuffer, InstanceData.SizeInBytes, 0);
}
// Copy the instance data over to the instance buffer
InstanceData[] instanceArray = s.InstanceDataListArray;
if (instanceArray.Length != instanceCount)
{
instanceArray = new InstanceData[instanceCount];
s.InstanceDataListArray = instanceArray;
}
s.InstanceDataList.CopyTo(instanceArray);
DataBox db = device.ImmediateContext.MapSubresource(s.InstanceDataBuffer, 0, MapMode.WriteDiscard, SharpDX.Direct3D11.MapFlags.None);
SharpDX.Utilities.Write(db.DataPointer, instanceArray, 0, instanceArray.Length);
device.ImmediateContext.UnmapSubresource(s.InstanceDataBuffer, 0);
}
if (removeList.Count != 0)
{
for (i = removeList.Count - 1; i >= 0; i--)
{
shapes.Remove(removeList[i]);
}
removeList.Clear();
}
}
/// 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);
}
}
}
/// 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 virtual void RenderScenePassMultiWorld(int pass, GameTime gameTime, DiscreteDynamicsWorld world)
{
IndexedMatrix m = IndexedMatrix.Identity;
IndexedBasisMatrix rot = IndexedBasisMatrix.Identity;
int numObjects = world.GetNumCollisionObjects();
IndexedVector3 wireColor = new IndexedVector3(1, 0, 0);
for (int i = 0; i < numObjects; i++)
{
CollisionObject colObj = world.GetCollisionObjectArray()[i];
RigidBody body = RigidBody.Upcast(colObj);
if (body != null && body.GetMotionState() != null)
{
DefaultMotionState myMotionState = (DefaultMotionState)body.GetMotionState();
//myMotionState.m_graphicsWorldTrans.getOpenGLMatrix(m);
m = myMotionState.m_graphicsWorldTrans;
rot = myMotionState.m_graphicsWorldTrans._basis;
}
else
{
//colObj.getWorldTransform().getOpenGLMatrix(m);
m = colObj.GetWorldTransform();
rot = colObj.GetWorldTransform()._basis;
}
wireColor = new IndexedVector3(1.0f, 1.0f, 0.5f); //wants deactivation
if ((i & 1) != 0)
{
wireColor = new IndexedVector3(0f, 0f, 1f);
}
///color differently for active, sleeping, wantsdeactivation states
if (colObj.GetActivationState() == ActivationState.ACTIVE_TAG) //active
{
if ((i & 1) != 0)
{
wireColor += new IndexedVector3(1f, 0f, 0f);
}
else
{
wireColor += new IndexedVector3(.5f, 0f, 0f);
}
}
if (colObj.GetActivationState() == ActivationState.ISLAND_SLEEPING) //ISLAND_SLEEPING
{
if ((i & 1) != 0)
{
wireColor += new IndexedVector3(0f, 1f, 0f);
}
else
{
wireColor += new IndexedVector3(0f, 05f, 0f);
}
}
IndexedVector3 min, max;
world.GetBroadphase().GetBroadphaseAabb(out min, out max);
min -= MathUtil.MAX_VECTOR;
max += MathUtil.MAX_VECTOR;
// printf("aabbMin=(%f,%f,%f)\n",aabbMin.getX(),aabbMin.getY(),aabbMin.getZ());
// printf("aabbMax=(%f,%f,%f)\n",aabbMax.getX(),aabbMax.getY(),aabbMax.getZ());
// m_dynamicsWorld.getDebugDrawer().drawAabb(aabbMin,aabbMax,btVector3(1,1,1));
switch (pass)
{
case 0:
{
m_shapeDrawer.DrawXNA(ref m, colObj.GetCollisionShape(), ref wireColor, m_debugDraw.GetDebugMode(), ref min, ref max, ref m_lookAt, ref m_perspective);
break;
}
case 1:
{
IndexedVector3 shadow = rot * m_lightDirection;
m_shapeDrawer.DrawShadow(ref m, ref shadow, colObj.GetCollisionShape(), ref min, ref max);
break;
}
case 2:
{
IndexedVector3 adjustedWireColor = wireColor * 0.3f;
m_shapeDrawer.DrawXNA(ref m, colObj.GetCollisionShape(), ref adjustedWireColor, 0, ref min, ref max, ref m_lookAt, ref m_perspective);
break;
}
}
}
}
public void InitInstancedRender(AlignedCollisionObjectArray objects)
{
// Clear instance data
foreach (ShapeData s in shapes.Values)
{
s.InstanceDataList.Clear();
}
int i = objects.Count - 1;
for (; i >= 0; i--)
{
CollisionObject colObj = objects[i];
BulletSharp.Math.Matrix transform;
if (colObj is SoftBody)
{
if (demo.IsDebugDrawEnabled)
{
continue;
}
transform = BulletSharp.Math.Matrix.Identity;
}
else
{
colObj.GetWorldTransform(out transform);
}
InitInstanceData(colObj, colObj.CollisionShape, ref transform);
}
foreach (KeyValuePair <CollisionShape, ShapeData> sh in shapes)
{
ShapeData s = sh.Value;
if (s.InstanceDataList.Count == 0)
{
removeList.Add(sh.Key);
}
/*
* // Is the instance buffer the right size?
* if (s.InstanceDataBuffer.Description.SizeInBytes != s.InstanceDataList.Count * InstanceData.SizeInBytes)
* {
* // No, recreate it
* s.InstanceDataBuffer.Dispose();
*
* if (s.InstanceDataList.Count == 0)
* {
* if (s.IndexBuffer != null)
* s.IndexBuffer.Dispose();
* s.VertexBuffer.Dispose();
* removeList.Add(sh.Key);
* continue;
* }
*
* instanceDataDesc.SizeInBytes = s.InstanceDataList.Count * InstanceData.SizeInBytes;
* s.InstanceDataBuffer = new Buffer(device, instanceDataDesc);
* s.BufferBindings[1] = new VertexBufferBinding(s.InstanceDataBuffer, InstanceData.SizeInBytes, 0);
* }
*
* // Copy the instance data over to the instance buffer
* using (var data = s.InstanceDataBuffer.Map(MapMode.WriteDiscard))
* {
* data.WriteRange(s.InstanceDataList.ToArray());
* s.InstanceDataBuffer.Unmap();
* }
*/
}
if (removeList.Count != 0)
{
for (i = removeList.Count - 1; i >= 0; i--)
{
shapes.Remove(removeList[i]);
}
removeList.Clear();
}
}
public override void ReportContacts(ChContactContainer mcontactcontainer)
{
// This should remove all old contacts (or at least rewind the index)
mcontactcontainer.BeginAddContact();
// NOTE: Bullet does not provide information on radius of curvature at a contact point.
// As such, for all Bullet-identified contacts, the default value will be used (SMC only).
ChCollisionInfo icontact = new ChCollisionInfo();
int numManifolds = bt_collision_world.GetDispatcher().GetNumManifolds();
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold contactManifold = bt_collision_world.GetDispatcher().GetManifoldByIndexInternal(i);
CollisionObject obA = (CollisionObject)(contactManifold.GetBody0());
CollisionObject obB = (CollisionObject)(contactManifold.GetBody1());
if (obA != null && obA != null) // Alan
{
contactManifold.RefreshContactPoints(ref obA.GetWorldTransform(), ref obB.GetWorldTransform());
icontact.modelA = (ChCollisionModel)obA.GetUserPointer();
icontact.modelB = (ChCollisionModel)obB.GetUserPointer();
double envelopeA = icontact.modelA.GetEnvelope();
double envelopeB = icontact.modelB.GetEnvelope();
double marginA = icontact.modelA.GetSafeMargin();
double marginB = icontact.modelB.GetSafeMargin();
// Execute custom broadphase callback, if any
bool do_narrow_contactgeneration = true;
if (this.broad_callback != null)
{
do_narrow_contactgeneration = this.broad_callback.OnBroadphase(icontact.modelA, icontact.modelB);
}
if (do_narrow_contactgeneration)
{
int numContacts = contactManifold.GetNumContacts();
//GetLog() << "numContacts=" << numContacts << "\n";
for (int j = 0; j < numContacts; j++)
{
// Debug.Log("contacts " + numContacts);
ManifoldPoint pt = contactManifold.GetContactPoint(j);
// Discard "too far" constraints (the Bullet engine also has its threshold)
if (pt.GetDistance() < marginA + marginB)
{
IndexedVector3 ptA = pt.GetPositionWorldOnA();
IndexedVector3 ptB = pt.GetPositionWorldOnB();
icontact.vpA.Set(ptA.X, ptA.Y, ptA.Z);
icontact.vpB.Set(ptB.X, ptB.Y, ptB.Z);
icontact.vN.Set(-pt.GetNormalWorldOnB().X, -pt.GetNormalWorldOnB().Y,
-pt.GetNormalWorldOnB().Z);
icontact.vN.Normalize();
double ptdist = pt.GetDistance();
icontact.vpA = icontact.vpA - icontact.vN * envelopeA;
icontact.vpB = icontact.vpB + icontact.vN * envelopeB;
icontact.distance = ptdist + envelopeA + envelopeB;
icontact.reaction_cache = pt.reactions_cache;// reactions_cache;
// Execute some user custom callback, if any
bool add_contact = true;
if (this.narrow_callback != null)
{
add_contact = this.narrow_callback.OnNarrowphase(icontact);
}
// Add to contact container
if (add_contact)
{
mcontactcontainer.AddContact(icontact);
}
}
}
}
}
// you can un-comment out this line, and then all points are removed
// contactManifold->clearManifold();
}
mcontactcontainer.EndAddContact();
}
public void InitInstancedRender(AlignedCollisionObjectArray objects)
{
// Clear instance data
foreach (ShapeData s in shapes.Values)
s.InstanceDataList.Clear();
removeList.Clear();
int i = objects.Count - 1;
for (; i >= 0; i--)
{
CollisionObject colObj = objects[i];
Matrix transform;
if (colObj is RigidBody)
{
DefaultMotionState motionState = (colObj as RigidBody).MotionState as DefaultMotionState;
if (motionState != null)
{
// FIXME: doesn't work with ConvexHullShape?
transform = motionState.GraphicsWorldTrans;
//colObj.GetWorldTransform(out transform);
}
else
{
colObj.GetWorldTransform(out transform);
}
}
else if (colObj is SoftBody)
{
if (demo.IsDebugDrawEnabled)
continue;
transform = BulletSharp.Matrix.Identity;
}
else
{
colObj.GetWorldTransform(out transform);
}
InitInstanceData(colObj, colObj.CollisionShape, ref transform);
}
foreach (KeyValuePair<CollisionShape, ShapeData> sh in shapes)
{
ShapeData s = sh.Value;
int instanceCount = s.InstanceDataList.Count;
// Is the instance buffer the right size?
if (s.InstanceDataBuffer.Description.SizeInBytes != instanceCount * InstanceData.SizeInBytes)
{
// No, recreate it
s.InstanceDataBuffer.Dispose();
if (instanceCount == 0)
{
if (s.IndexBuffer != null)
s.IndexBuffer.Dispose();
s.VertexBuffer.Dispose();
removeList.Add(sh.Key);
continue;
}
instanceDataDesc.SizeInBytes = instanceCount * InstanceData.SizeInBytes;
s.InstanceDataBuffer = new Buffer(device, instanceDataDesc);
s.BufferBindings[1] = new VertexBufferBinding(s.InstanceDataBuffer, InstanceData.SizeInBytes, 0);
}
// Copy the instance data over to the instance buffer
InstanceData[] instanceArray = s.InstanceDataListArray;
if (instanceArray.Length != instanceCount)
{
instanceArray = new InstanceData[instanceCount];
s.InstanceDataListArray = instanceArray;
}
s.InstanceDataList.CopyTo(instanceArray);
using (var data = s.InstanceDataBuffer.Map(MapMode.WriteDiscard, global::SharpDX.Direct3D10.MapFlags.None))
{
data.WriteRange(instanceArray);
s.InstanceDataBuffer.Unmap();
}
}
if (removeList.Count != 0)
{
for (i = removeList.Count - 1; i >= 0; i--)
{
shapes.Remove(removeList[i]);
}
}
}
