public override void ProcessAllTriangles(ITriangleCallback callback, ref Vector3 aabbMin, ref Vector3 aabbMax)
{
Vector3 halfExtents = (aabbMax - aabbMin) * .5f;
float radius = halfExtents.Length();
Vector3 center = (aabbMax + aabbMin) * 0.5f;
//this is where the triangles are generated, given AABB and plane equation (normal/constant)
Vector3 tangentDir0 = Vector3.Zero;
Vector3 tangentDir1 = Vector3.Zero;
//tangentDir0/tangentDir1 can be precalculated
TransformUtil.PlaneSpace1(ref m_planeNormal,ref tangentDir0,ref tangentDir1);
Vector3 supVertex0 = Vector3.Zero;
Vector3 supVertex1 = Vector3.Zero;
Vector3 projectedCenter = center - (Vector3.Dot(m_planeNormal,center) - m_planeConstant)*m_planeNormal;
ObjectArray<Vector3> triangle = new ObjectArray<Vector3>(3);
triangle.Add(projectedCenter + tangentDir0*radius + tangentDir1*radius);
triangle.Add(projectedCenter + tangentDir0 * radius - tangentDir1 * radius);
triangle.Add(projectedCenter - tangentDir0 * radius - tangentDir1 * radius);
callback.ProcessTriangle(triangle,0,0);
triangle[0] = projectedCenter - tangentDir0*radius - tangentDir1*radius;
triangle[1] = projectedCenter - tangentDir0*radius + tangentDir1*radius;
triangle[2] = projectedCenter + tangentDir0*radius + tangentDir1*radius;
callback.ProcessTriangle(triangle,0,1);
}
public void AddValue()
{
var sut = new ObjectArray();
sut.Add(0);
sut.Add(2.657);
sut.Add("string");
Assert.Equal("string", sut[2]);
Assert.Equal(3, sut.Count);
}
public void CheckLengthAferInsertion()
{
var sut = new ObjectArray();
var intArr = new int[] { 2, 3, 4 };
sut.Add(0);
sut.Add(true);
sut.Add(3.4647);
sut.Add("string");
sut.Add(intArr);
sut.Insert(1, 4);
Assert.Equal(6, sut.Count);
}
///the clipFace method is used internally
public static void ClipFace(ObjectArray <IndexedVector3> pVtxIn, ObjectArray <IndexedVector3> ppVtxOut, ref IndexedVector3 planeNormalWS, float planeEqWS)
{
int ve;
float ds, de;
int numVerts = pVtxIn.Count;
if (numVerts < 2)
{
return;
}
IndexedVector3 firstVertex = pVtxIn[pVtxIn.Count - 1];
IndexedVector3 endVertex = pVtxIn[0];
IndexedVector3.Dot(ref planeNormalWS, ref firstVertex, out ds);
ds += planeEqWS;
for (ve = 0; ve < numVerts; ve++)
{
endVertex = pVtxIn[ve];
IndexedVector3.Dot(ref planeNormalWS, ref endVertex, out de);
de += planeEqWS;
if (ds < 0)
{
if (de < 0)
{
// Start < 0, end < 0, so output endVertex
ppVtxOut.Add(endVertex);
}
else
{
// Start < 0, end >= 0, so output intersection
ppVtxOut.Add(MathUtil.Vector3Lerp(ref firstVertex, ref endVertex, (float)(ds * 1.0f / (ds - de))));
}
}
else
{
if (de < 0)
{
// Start >= 0, end < 0 so output intersection and end
ppVtxOut.Add(MathUtil.Vector3Lerp(ref firstVertex, ref endVertex, (float)(ds * 1.0f / (ds - de))));
ppVtxOut.Add(endVertex);
}
}
firstVertex = endVertex;
ds = de;
}
}
CollisionShape BuildLargeMesh()
{
//int vertStride = sizeof(IndexedVector3);
//int indexStride = 3*sizeof(int);
int vertStride = 1;
int indexStride = 3;
ObjectArray <IndexedVector3> vertexArray = new ObjectArray <IndexedVector3>();
for (int i = 0; i < vertices.Length; ++i)
{
vertexArray.Add(vertices[i]);
}
ObjectArray <int> intArray = new ObjectArray <int>();
for (int i = 0; i < indices.Length; ++i)
{
intArray.Add(indices[i]);
}
//TriangleIndexVertexArray indexVertexArray = new TriangleIndexVertexArray(DemoMeshes.BUNNY_NUM_TRIANGLES, DemoMeshes.gBunnyIndices, 3, DemoMeshes.BUNNY_NUM_VERTICES, DemoMeshes.gBunnyVertices, 3);
TriangleIndexVertexArray indexVertexArray = new TriangleIndexVertexArray(numTriangles, intArray, indexStride, vertexArray.Count, vertexArray, vertStride);
TriangleMeshShape triangleMesh = new TriangleMeshShape(indexVertexArray);
//TriangleMeshShape triangleMesh = new BvhTriangleMeshShape(indexVertexArray,true,true);
return(triangleMesh);
}
public void QueryNode(QuadTreeNode node, ObjectArray <QuadTreeNode> results, IndexedVector3 source, IndexedVector3 direction)
{
//if(node.children == null && node.Intersects(raySource,rayTarget))
// add the lowest level.
if (node.children == null)
{
results.Add(node);
#if DEBUG_ACCELERATOR
if (BulletGlobals.gDebugDraw != null)
{
IndexedVector3 drawMin = new IndexedVector3(node.boundingBox.Min);
IndexedVector3 drawMax = new IndexedVector3(node.boundingBox.Max);
IndexedVector3 worldMin = LocalToWorld2(drawMin);
IndexedVector3 worldMax = LocalToWorld2(drawMax);
BulletGlobals.gDebugDraw.DrawAabb(worldMin, worldMax, new IndexedVector3(1, 1, 1));
}
#endif
}
else
{
// simple rescursive for now.
for (int i = 0; i < 4; ++i)
{
if (node.children[i].Intersects(source, direction))
{
QueryNode(node.children[i], results, source, direction);
}
}
}
}
public void ContainsElement()
{
var intArr = new int[] { 2, 3, 4 };
var sut = new ObjectArray();
sut.Add(0);
sut.Add(true);
sut.Add(3.4647);
sut.Add("string");
sut.Add(intArr);
sut.Add(6);
sut.Add(0);
sut.Add(8);
sut.Add(9);
sut.Add(10);
Assert.True(sut.Contains(intArr));
}
public void CountElements()
{
var intArr = new int[] { 2, 3, 4 };
var sut = new ObjectArray();
sut.Add(0);
sut.Add(true);
sut.Add(3.4647);
sut.Add("string");
sut.Add(intArr);
sut.Add(6);
sut.Add(0);
sut.Add(8);
sut.Add(9);
sut.Add(10);
Assert.Equal(10, sut.Count);
}
protected void BuildMeshParts(StridingMeshInterface meshInterface)
{
for (int i = 0; i < meshInterface.GetNumSubParts(); ++i)
{
GImpactMeshShapePart newpart = new GImpactMeshShapePart(meshInterface, i);
m_mesh_parts.Add(newpart);
}
}
public void IndexOfElement()
{
var intArr = new int[] { 2, 3, 4 };
var sut = new ObjectArray();
sut.Add(0);
sut.Add(true);
sut.Add(3.4647);
sut.Add("string");
sut.Add(intArr);
sut.Add(6);
sut.Add(0);
sut.Add(8);
sut.Add(9);
sut.Add(10);
Assert.Equal(3, sut.IndexOf("string"));
}
public void VerifyCountAfterRemove()
{
var sut = new ObjectArray();
var intArr = new int[] { 2, 3, 4 };
sut.Add(0);
sut.Add(true);
sut.Add(3.4647);
sut.Add("string");
sut.Add(intArr);
sut.Add(6);
sut.Add(0);
sut.Add(8);
sut.Add(9);
sut.Add(10);
sut.RemoveAt(2);
Assert.Equal(9, sut.Count);
}
public void InsertAndResize()
{
var sut = new ObjectArray();
var intArr = new int[] { 2, 3, 4 };
sut.Add(0);
sut.Add(true);
sut.Add(3.4647);
sut.Add("string");
sut.Add(intArr);
sut.Add(6);
sut.Add(0);
sut.Add(8);
sut.Add(9);
sut.Add(10);
sut.Insert(3, true);
Assert.Equal(4, sut.IndexOf("string"));
}
public bool BoxQuery(ref AABB box, ObjectArray <int> collided_results, bool graphics)
{
int curIndex = 0;
int numNodes = GetNodeCount();
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactBVH && !graphics)
{
BulletGlobals.g_streamWriter.WriteLine("QIQBVH BoxQuery [{0}]", numNodes);
}
//quantize box
UShortVector3 quantizedMin;
UShortVector3 quantizedMax;
m_box_tree.QuantizePoint(out quantizedMin, ref box.m_min);
m_box_tree.QuantizePoint(out quantizedMax, ref box.m_max);
while (curIndex < numNodes)
{
//catch bugs in tree data
bool aabbOverlap = m_box_tree.TestQuantizedBoxOverlap(curIndex, ref quantizedMin, ref quantizedMax);
bool isLeafNode = IsLeafNode(curIndex);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactBVH && !graphics)
{
BulletGlobals.g_streamWriter.WriteLine("QIQBVH BoxQuery [{0}] o[{1}] l[{2}]", curIndex, aabbOverlap ? 1 : 0, isLeafNode ? 1 : 0);
}
if (isLeafNode && aabbOverlap)
{
foreach (var i in GetNodeData(curIndex))
{
collided_results.Add(i);
}
}
if (aabbOverlap || isLeafNode)
{
//next subnode
curIndex++;
}
else
{
//skip node
curIndex += GetEscapeNodeIndex(curIndex);
}
}
if (collided_results.Count > 0)
{
return(true);
}
return(false);
}
public void GetElementsAtIndex()
{
var intArr = new int[] { 2, 3, 4 };
var sut = new ObjectArray();
sut.Add(0);
sut.Add(true);
sut.Add(3.4647);
sut.Add("string");
sut.Add(intArr);
sut.Add(6);
sut.Add(0);
sut.Add(8);
sut.Add(9);
sut.Add(10);
Assert.Equal(intArr, sut[4]);
}
public void VerifyElementsAfterRemove()
{
var sut = new ObjectArray();
var intArr = new int[] { 2, 3, 4 };
sut.Add(0);
sut.Add(true);
sut.Add(3.4647);
sut.Add("string");
sut.Add(intArr);
sut.Add(6);
sut.Add(0);
sut.Add(8);
sut.Add(9);
sut.Add(10);
sut.RemoveAt(1);
Assert.Equal("string", sut[2]);
Assert.Equal(intArr, sut[3]);
}
public void AddValueAndResize()
{
var intArr = new int[] { 2, 3, 4 };
var sut = new ObjectArray();
sut.Add(0);
sut.Add(true);
sut.Add(3.4647);
sut.Add("string");
sut.Add(intArr);
sut.Add(6);
sut.Add(0);
sut.Add(8);
sut.Add(9);
sut.Add(10);
Assert.Equal(intArr, sut[4]);
Assert.Equal(10, sut.Count);
}
public void IndexOfElementNotFound()
{
string str = "abc";
object s = str;
var intArr = new int[] { 2, 3, 4 };
var sut = new ObjectArray();
sut.Add(0);
sut.Add(true);
sut.Add(3.4647);
sut.Add("string");
sut.Add(intArr);
sut.Add(6);
sut.Add(0);
sut.Add(8);
sut.Add(9);
sut.Add(10);
Assert.Equal(-1, sut.IndexOf(s));
}
public void ChangeValueAtIndex()
{
string s = "new string";
object stringObject = s;
var sut = new ObjectArray();
var intArr = new int[] { 2, 3, 4 };
sut.Add(0);
sut.Add(true);
sut.Add(3.4647);
sut.Add("string");
sut.Add(intArr);
sut.Add(6);
sut.Add(0);
sut.Add(8);
sut.Add(9);
sut.Add(10);
sut[1] = stringObject;
Assert.Equal("new string", sut[1]);
}
public void IndexOfElement()
{
int val = 9;
object objValue = val;
var sut = new ObjectArray();
var intArr = new int[] { 2, 3, 4 };
sut.Add(0);
sut.Add(true);
sut.Add(3.4647);
sut.Add("string");
sut.Add(intArr);
sut.Add(6);
sut.Add(0);
sut.Add(8);
sut.Add(9);
sut.Add(10);
sut.Insert(1, objValue);
Assert.Equal(1, sut.IndexOf(objValue));
}
public void NotContainsElement()
{
int valueType = 57585;
object elem = valueType;
var intArr = new int[] { 2, 3, 4 };
var sut = new ObjectArray();
sut.Add(0);
sut.Add(true);
sut.Add(3.4647);
sut.Add("string");
sut.Add(intArr);
sut.Add(6);
sut.Add(0);
sut.Add(8);
sut.Add(9);
sut.Add(10);
Assert.False(sut.Contains(elem));
}
public static void FetchLeafs(Dbvt pdbvt, DbvtNode root, ObjectArray <DbvtNode> leafs, int depth)
{
if (root.IsInternal() && depth != 0)
{
FetchLeafs(pdbvt, root._children[0], leafs, depth - 1);
FetchLeafs(pdbvt, root._children[1], leafs, depth - 1);
DeleteNode(pdbvt, root);
}
else
{
leafs.Add(root);
}
}
public static void Split(ObjectArray <DbvtNode> leaves, ObjectArray <DbvtNode> left, ObjectArray <DbvtNode> right, ref IndexedVector3 org, ref IndexedVector3 axis)
{
left.Resize(0);
right.Resize(0);
for (int i = 0, ni = leaves.Count; i < ni; ++i)
{
if (IndexedVector3.Dot(axis, leaves[i].volume.Center() - org) < 0)
{
left.Add(leaves[i]);
}
else
{
right.Add(leaves[i]);
}
}
}
private bool BoxQuery(ref AABB box, ObjectArray <int> collided_results, bool graphics)
{
int curIndex = 0;
int numNodes = GetNodeCount();
//quantize box
UShortVector3 quantizedMin;
UShortVector3 quantizedMax;
m_box_tree.QuantizePoint(out quantizedMin, ref box.m_min);
m_box_tree.QuantizePoint(out quantizedMax, ref box.m_max);
while (curIndex < numNodes)
{
//catch bugs in tree data
bool aabbOverlap = m_box_tree.TestQuantizedBoxOverlap(curIndex, ref quantizedMin, ref quantizedMax);
bool isLeafNode = IsLeafNode(curIndex);
if (isLeafNode && aabbOverlap)
{
foreach (var i in GetNodeData(curIndex))
{
collided_results.Add(i);
}
}
if (aabbOverlap || isLeafNode)
{
//next subnode
curIndex++;
}
else
{
//skip node
curIndex += GetEscapeNodeIndex(curIndex);
}
}
if (collided_results.Count > 0)
{
return(true);
}
return(false);
}
protected void GImpactVsShapeFindPairs(
ref IndexedMatrix trans0,
ref IndexedMatrix trans1,
GImpactShapeInterface shape0,
CollisionShape shape1,
ObjectArray <int> collided_primitives)
{
AABB boxshape = new AABB();
if (shape0.HasBoxSet())
{
IndexedMatrix trans1to0 = trans0.Inverse();
//trans1to0 *= trans1;
trans1to0 = trans1to0 * trans1;
//trans1to0 = MathUtil.BulletMatrixMultiply(trans1,trans1to0);
shape1.GetAabb(ref trans1to0, out boxshape.m_min, out boxshape.m_max);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "GImpactAglo::GImpactVsShapeFindPairs trans1to0", trans1to0);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box min", boxshape.m_min);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box max", boxshape.m_max);
}
#endif
shape0.GetBoxSet().BoxQuery(ref boxshape, collided_primitives);
}
else
{
shape1.GetAabb(ref trans1, out boxshape.m_min, out boxshape.m_max);
AABB boxshape0 = new AABB();
int i = shape0.GetNumChildShapes();
while (i-- != 0)
{
shape0.GetChildAabb(i, ref trans0, out boxshape0.m_min, out boxshape0.m_max);
if (boxshape.HasCollision(ref boxshape0))
{
collided_primitives.Add(i);
}
}
}
}
public static void ClipHullAgainstHull(ref IndexedVector3 separatingNormal1, ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
IndexedVector3 separatingNormal = separatingNormal1.Normalized();
IndexedVector3 c0 = transA * hullA.m_localCenter;
IndexedVector3 c1 = transB * hullB.m_localCenter;
IndexedVector3 DeltaC2 = c0 - c1;
float curMaxDist = maxDist;
int closestFaceB = -1;
float dmax = float.MinValue;
{
for (int face = 0; face < hullB.m_faces.Count; face++)
{
IndexedVector3 Normal = new IndexedVector3(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
IndexedVector3 WorldNormal = transB._basis * Normal;
float d = IndexedVector3.Dot(WorldNormal, separatingNormal);
if (d > dmax)
{
dmax = d;
closestFaceB = face;
}
}
}
// setup initial clip face (minimizing face from hull B)
ObjectArray <IndexedVector3> worldVertsB1 = new ObjectArray <IndexedVector3>();
{
Face polyB = hullB.m_faces[closestFaceB];
int numVertices = polyB.m_indices.Count;
for (int e0 = 0; e0 < numVertices; e0++)
{
IndexedVector3 b = hullB.m_vertices[polyB.m_indices[e0]];
// check this to see if it is transposed version
worldVertsB1.Add(transB * b);
}
}
if (closestFaceB >= 0)
{
ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut);
}
}
//! returns the indices of the primitives in the m_primitive_manager
public bool RayQuery(ref IndexedVector3 ray_dir, ref IndexedVector3 ray_origin,
ObjectArray <int> collided_results)
{
int curIndex = 0;
int numNodes = GetNodeCount();
while (curIndex < numNodes)
{
AABB bound;
GetNodeBound(curIndex, out bound);
//catch bugs in tree data
bool aabbOverlap = bound.CollideRay(ref ray_origin, ref ray_dir);
bool isLeafNode = IsLeafNode(curIndex);
if (isLeafNode && aabbOverlap)
{
foreach (var i in GetNodeData(curIndex))
{
collided_results.Add(i);
}
}
if (aabbOverlap || isLeafNode)
{
//next subnode
curIndex++;
}
else
{
//skip node
curIndex += GetEscapeNodeIndex(curIndex);
}
}
if (collided_results.Count > 0)
{
return(true);
}
return(false);
}
public CollisionShape BuildCorner()
{
// slope.
IndexedVector3[] vertices = new IndexedVector3[] { new IndexedVector3(0, 0, 0), new IndexedVector3(1, 0, 0), new IndexedVector3(0, 0, 1), new IndexedVector3(1, 0, 1),
new IndexedVector3(0, 1, 0), new IndexedVector3(1, 1, 0), new IndexedVector3(0, 1, 1), new IndexedVector3(1, 1, 1) };
//int[] indices = new int[] { 0, 4, 5, 4, 6, 7, 7, 5, 4, 0, 4, 6, 6, 2, 0, 2, 6, 7, 4,5,0,2,2,7,5};
//int[] indices = new int[] { 0, 4, 5, 4, 6, 7, 7, 5, 4, 6, 4,0,0,2,6, 7, 6, 2, 4, 5, 0, 2, 2, 7, 5 };
int[] indices = new int[] { 1, 4, 5,
1, 5, 7,
7, 3, 1,
3, 7, 6,
7, 5, 4,
4, 6, 7,
4, 1, 3,
3, 6, 4 };
int vertStride = 1;
int indexStride = 3;
ObjectArray <IndexedVector3> vertexArray = new ObjectArray <IndexedVector3>();
for (int i = 0; i < vertices.Length; ++i)
{
vertexArray.Add(vertices[i]);
}
ObjectArray <int> intArray = new ObjectArray <int>();
for (int i = 0; i < indices.Length; ++i)
{
intArray.Add(indices[i]);
}
TriangleIndexVertexArray indexVertexArray = new TriangleIndexVertexArray(indices.Length / 3, intArray, indexStride, vertexArray.Count, vertexArray, vertStride);
TriangleMeshShape triangleMesh = new TriangleMeshShape(indexVertexArray);
//TriangleMeshShape triangleMesh = new BvhTriangleMeshShape(indexVertexArray,true,true);
return(triangleMesh);
}
public static CollisionShape BuildCorner(IndexedVector3[] vertices, int[] indices)
{
int vertStride = 1;
int indexStride = 3;
ObjectArray <IndexedVector3> vertexArray = new ObjectArray <IndexedVector3>();
for (int i = 0; i < vertices.Length; ++i)
{
vertexArray.Add(vertices[i]);
}
ObjectArray <int> intArray = new ObjectArray <int>();
for (int i = 0; i < indices.Length; ++i)
{
intArray.Add(indices[i]);
}
TriangleIndexVertexArray indexVertexArray = new TriangleIndexVertexArray(indices.Length / 3, intArray, indexStride, vertexArray.Count, vertexArray, vertStride);
TriangleMeshShape triangleMesh = new TriangleMeshShape(indexVertexArray);
return(triangleMesh);
}
//! returns the indices of the primitives in the m_primitive_manager
public bool BoxQuery(ref AABB box, ObjectArray <int> collided_results)
{
int curIndex = 0;
int numNodes = GetNodeCount();
while (curIndex < numNodes)
{
AABB bound = new AABB();
GetNodeBound(curIndex, out bound);
//catch bugs in tree data
bool aabbOverlap = bound.HasCollision(ref box);
bool isleafnode = IsLeafNode(curIndex);
if (isleafnode && aabbOverlap)
{
collided_results.Add(GetNodeData(curIndex));
}
if (aabbOverlap || isleafnode)
{
//next subnode
curIndex++;
}
else
{
//skip node
curIndex += GetEscapeNodeIndex(curIndex);
}
}
if (collided_results.Count > 0)
{
return(true);
}
return(false);
}
public static void ResizeSolverConstraintList(ObjectArray<SolverConstraint> list, int newSize)
{
int listSize = list.Count;
int sizeDiff = newSize - listSize;
// grow if needed
if (listSize < newSize)
{
for (int i = 0; i < sizeDiff; ++i)
{
list.Add(new SolverConstraint());
}
}
else
{
// Trim down
for (int i = sizeDiff; i < 0; ++i)
{
list.RemoveAt(list.Count - 1);
}
}
}
///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges)
public virtual bool InitializePolyhedralFeatures()
{
#if USE_CONVEX_HULL_COMPUTER
if (m_polyhedron != null)
{
m_polyhedron = null;
}
m_polyhedron = new ConvexPolyhedron();
ObjectArray <IndexedVector3> tmpVertices = new ObjectArray <IndexedVector3>();
for (int i = 0; i < GetNumVertices(); i++)
{
IndexedVector3 newVertex;
GetVertex(i, out newVertex);
tmpVertices.Add(newVertex);
}
ConvexHullComputer conv = new ConvexHullComputer();
//conv.compute(&tmpVertices[0].getX(), sizeof(IndexedVector3),tmpVertices.Count,0.0f,0.0f);
conv.Compute(tmpVertices, 0, tmpVertices.Count, 0.0f, 0.0f);
ObjectArray <IndexedVector3> faceNormals = new ObjectArray <IndexedVector3>();
int numFaces = conv.faces.size();
faceNormals.Resize(numFaces);
ConvexHullComputer convexUtil = conv;
m_polyhedron.m_faces.Resize(numFaces);
int numVertices = convexUtil.vertices.Count;
m_polyhedron.m_vertices.Resize(numVertices);
for (int p = 0; p < numVertices; p++)
{
m_polyhedron.m_vertices[p] = convexUtil.vertices[p];
}
for (int i = 0; i < numFaces; i++)
{
int face = convexUtil.faces[i];
//printf("face=%d\n",face);
Edge firstEdge = convexUtil.edges[face];
Edge edge = firstEdge;
IndexedVector3[] edges = new IndexedVector3[3];
int numEdges = 0;
//compute face normals
float maxCross2 = 0.0f;
int chosenEdge = -1;
do
{
int src = edge.GetSourceVertex();
m_polyhedron.m_faces[i].m_indices.Add(src);
int targ = edge.GetTargetVertex();
IndexedVector3 wa = convexUtil.vertices[src];
IndexedVector3 wb = convexUtil.vertices[targ];
IndexedVector3 newEdge = wb - wa;
newEdge.Normalize();
if (numEdges < 2)
{
edges[numEdges++] = newEdge;
}
edge = edge.GetNextEdgeOfFace();
} while (edge != firstEdge);
float planeEq = 1e30f;
if (numEdges == 2)
{
faceNormals[i] = IndexedVector3.Cross(edges[0], edges[1]);
faceNormals[i].Normalize();
m_polyhedron.m_faces[i].m_plane[0] = -faceNormals[i].X;
m_polyhedron.m_faces[i].m_plane[1] = -faceNormals[i].Y;
m_polyhedron.m_faces[i].m_plane[2] = -faceNormals[i].Z;
m_polyhedron.m_faces[i].m_plane[3] = planeEq;
}
else
{
Debug.Assert(false);//degenerate?
faceNormals[i] = IndexedVector3.Zero;
}
for (int v = 0; v < m_polyhedron.m_faces[i].m_indices.Count; v++)
{
float eq = IndexedVector3.Dot(m_polyhedron.m_vertices[m_polyhedron.m_faces[i].m_indices[v]], faceNormals[i]);
if (planeEq > eq)
{
planeEq = eq;
}
}
m_polyhedron.m_faces[i].m_plane[3] = planeEq;
}
if (m_polyhedron.m_faces.Count > 0 && conv.vertices.Count > 0)
{
for (int f = 0; f < m_polyhedron.m_faces.Count; f++)
{
IndexedVector3 planeNormal = new IndexedVector3(m_polyhedron.m_faces[f].m_plane[0], m_polyhedron.m_faces[f].m_plane[1], m_polyhedron.m_faces[f].m_plane[2]);
float planeEq = m_polyhedron.m_faces[f].m_plane[3];
IndexedVector3 supVec = LocalGetSupportingVertex(-planeNormal);
if (IndexedVector3.Dot(supVec, planeNormal) < planeEq)
{
m_polyhedron.m_faces[f].m_plane[0] *= -1;
m_polyhedron.m_faces[f].m_plane[1] *= -1;
m_polyhedron.m_faces[f].m_plane[2] *= -1;
m_polyhedron.m_faces[f].m_plane[3] *= -1;
int numVerts = m_polyhedron.m_faces[f].m_indices.Count;
for (int v = 0; v < numVerts / 2; v++)
{
int temp = m_polyhedron.m_faces[f].m_indices[v];
m_polyhedron.m_faces[f].m_indices[v] = m_polyhedron.m_faces[f].m_indices[numVerts - 1 - v];
m_polyhedron.m_faces[f].m_indices[numVerts - 1 - v] = temp;
}
}
}
}
m_polyhedron.Initialize();
#endif
return(true);
}
public static CollisionShape BuildCorner(IndexedVector3[] vertices, int[] indices)
{
int vertStride = 1;
int indexStride = 3;
ObjectArray<IndexedVector3> vertexArray = new ObjectArray<IndexedVector3>();
for (int i = 0; i < vertices.Length; ++i)
{
vertexArray.Add(vertices[i]);
}
ObjectArray<int> intArray = new ObjectArray<int>();
for (int i = 0; i < indices.Length; ++i)
{
intArray.Add(indices[i]);
}
TriangleIndexVertexArray indexVertexArray = new TriangleIndexVertexArray(indices.Length / 3, intArray, indexStride, vertexArray.Count, vertexArray, vertStride);
TriangleMeshShape triangleMesh = new TriangleMeshShape(indexVertexArray);
return triangleMesh;
}
///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges)
public virtual bool InitializePolyhedralFeatures()
{
#if USE_CONVEX_HULL_COMPUTER
if (m_polyhedron != null)
{
m_polyhedron = null;
}
m_polyhedron = new ConvexPolyhedron();
ObjectArray<IndexedVector3> tmpVertices = new ObjectArray<IndexedVector3>();
for (int i = 0; i < GetNumVertices(); i++)
{
IndexedVector3 newVertex;
GetVertex(i, out newVertex);
tmpVertices.Add(newVertex);
}
ConvexHullComputer conv = new ConvexHullComputer();
//conv.compute(&tmpVertices[0].getX(), sizeof(IndexedVector3),tmpVertices.Count,0.0f,0.0f);
conv.Compute(tmpVertices, 0, tmpVertices.Count, 0.0f, 0.0f);
ObjectArray<IndexedVector3> faceNormals = new ObjectArray<IndexedVector3>();
int numFaces = conv.faces.size();
faceNormals.Resize(numFaces);
ConvexHullComputer convexUtil = conv;
m_polyhedron.m_faces.Resize(numFaces);
int numVertices = convexUtil.vertices.Count;
m_polyhedron.m_vertices.Resize(numVertices);
for (int p = 0; p < numVertices; p++)
{
m_polyhedron.m_vertices[p] = convexUtil.vertices[p];
}
for (int i = 0; i < numFaces; i++)
{
int face = convexUtil.faces[i];
//printf("face=%d\n",face);
Edge firstEdge = convexUtil.edges[face];
Edge edge = firstEdge;
IndexedVector3[] edges = new IndexedVector3[3];
int numEdges = 0;
//compute face normals
float maxCross2 = 0.0f;
int chosenEdge = -1;
do
{
int src = edge.GetSourceVertex();
m_polyhedron.m_faces[i].m_indices.Add(src);
int targ = edge.GetTargetVertex();
IndexedVector3 wa = convexUtil.vertices[src];
IndexedVector3 wb = convexUtil.vertices[targ];
IndexedVector3 newEdge = wb - wa;
newEdge.Normalize();
if (numEdges < 2)
{
edges[numEdges++] = newEdge;
}
edge = edge.GetNextEdgeOfFace();
} while (edge != firstEdge);
float planeEq = 1e30f;
if (numEdges == 2)
{
faceNormals[i] = IndexedVector3.Cross(edges[0], edges[1]);
faceNormals[i].Normalize();
m_polyhedron.m_faces[i].m_plane[0] = -faceNormals[i].X;
m_polyhedron.m_faces[i].m_plane[1] = -faceNormals[i].Y;
m_polyhedron.m_faces[i].m_plane[2] = -faceNormals[i].Z;
m_polyhedron.m_faces[i].m_plane[3] = planeEq;
}
else
{
Debug.Assert(false);//degenerate?
faceNormals[i] = IndexedVector3.Zero;
}
for (int v = 0; v < m_polyhedron.m_faces[i].m_indices.Count; v++)
{
float eq = IndexedVector3.Dot(m_polyhedron.m_vertices[m_polyhedron.m_faces[i].m_indices[v]], faceNormals[i]);
if (planeEq > eq)
{
planeEq = eq;
}
}
m_polyhedron.m_faces[i].m_plane[3] = planeEq;
}
if (m_polyhedron.m_faces.Count > 0 && conv.vertices.Count > 0)
{
for (int f = 0; f < m_polyhedron.m_faces.Count; f++)
{
IndexedVector3 planeNormal = new IndexedVector3(m_polyhedron.m_faces[f].m_plane[0], m_polyhedron.m_faces[f].m_plane[1], m_polyhedron.m_faces[f].m_plane[2]);
float planeEq = m_polyhedron.m_faces[f].m_plane[3];
IndexedVector3 supVec = LocalGetSupportingVertex(-planeNormal);
if (IndexedVector3.Dot(supVec, planeNormal) < planeEq)
{
m_polyhedron.m_faces[f].m_plane[0] *= -1;
m_polyhedron.m_faces[f].m_plane[1] *= -1;
m_polyhedron.m_faces[f].m_plane[2] *= -1;
m_polyhedron.m_faces[f].m_plane[3] *= -1;
int numVerts = m_polyhedron.m_faces[f].m_indices.Count;
for (int v = 0; v < numVerts / 2; v++)
{
int temp = m_polyhedron.m_faces[f].m_indices[v];
m_polyhedron.m_faces[f].m_indices[v] = m_polyhedron.m_faces[f].m_indices[numVerts - 1 - v];
m_polyhedron.m_faces[f].m_indices[numVerts - 1 - v] = temp;
}
}
}
}
m_polyhedron.Initialize();
#endif
return true;
}
public static void FetchLeafs(Dbvt pdbvt, DbvtNode root, ObjectArray<DbvtNode> leafs, int depth)
{
if (root.IsInternal() && depth != 0)
{
FetchLeafs(pdbvt, root._children[0], leafs, depth - 1);
FetchLeafs(pdbvt, root._children[1], leafs, depth - 1);
DeleteNode(pdbvt, root);
}
else
{
leafs.Add(root);
}
}
/// process all triangles within the provided axis-aligned bounding box
/**
basic algorithm:
- convert input aabb to local coordinates (scale down and shift for local origin)
- convert input aabb to a range of heightfield grid points (quantize)
- iterate over all triangles in that subset of the grid
*/
public override void ProcessAllTriangles(ITriangleCallback callback, ref Vector3 aabbMin, ref Vector3 aabbMax)
{
// scale down the input aabb's so they are in local (non-scaled) coordinates
Vector3 invScale = new Vector3(1f, 1f, 1f);
invScale /= m_localScaling;
Vector3 localAabbMin = aabbMin * invScale;
Vector3 localAabbMax = aabbMax * invScale;
// account for local origin
localAabbMin += m_localOrigin;
localAabbMax += m_localOrigin;
//quantize the aabbMin and aabbMax, and adjust the start/end ranges
int[] quantizedAabbMin = new int[3];
int[] quantizedAabbMax = new int[3];
QuantizeWithClamp(quantizedAabbMin, ref localAabbMin, 0);
QuantizeWithClamp(quantizedAabbMax, ref localAabbMax, 1);
// expand the min/max quantized values
// this is to catch the case where the input aabb falls between grid points!
for (int i = 0; i < 3; ++i)
{
quantizedAabbMin[i]--;
quantizedAabbMax[i]++;
}
int startX = 0;
int endX = m_heightStickWidth - 1;
int startJ = 0;
int endJ = m_heightStickLength - 1;
switch (m_upAxis)
{
case 0:
{
if (quantizedAabbMin[1] > startX)
startX = quantizedAabbMin[1];
if (quantizedAabbMax[1] < endX)
endX = quantizedAabbMax[1];
if (quantizedAabbMin[2] > startJ)
startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2] < endJ)
endJ = quantizedAabbMax[2];
break;
}
case 1:
{
if (quantizedAabbMin[0] > startX)
startX = quantizedAabbMin[0];
if (quantizedAabbMax[0] < endX)
endX = quantizedAabbMax[0];
if (quantizedAabbMin[2] > startJ)
startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2] < endJ)
endJ = quantizedAabbMax[2];
break;
};
case 2:
{
if (quantizedAabbMin[0] > startX)
startX = quantizedAabbMin[0];
if (quantizedAabbMax[0] < endX)
endX = quantizedAabbMax[0];
if (quantizedAabbMin[1] > startJ)
startJ = quantizedAabbMin[1];
if (quantizedAabbMax[1] < endJ)
endJ = quantizedAabbMax[1];
break;
}
default:
{
//need to get valid m_upAxis
Debug.Assert(false);
break;
}
}
ObjectArray<Vector3> vertices = new ObjectArray<Vector3>();
Vector3[] tempVectors = new Vector3[3];
for (int j = startJ; j < endJ; j++)
{
for (int x = startX; x < endX; x++)
{
if (m_flipQuadEdges || (m_useDiamondSubdivision && (((j + x) & 1) > 0)))
{
//first triangle
GetVertex(x, j, ref tempVectors[0]);
GetVertex(x + 1, j, ref tempVectors[1]);
GetVertex(x + 1, j + 1, ref tempVectors[2]);
vertices.Clear();
for (int a = 0; a < tempVectors.Length; ++a)
{
vertices.Add(tempVectors[a]);
}
callback.ProcessTriangle(vertices, x, j);
//second triangle
GetVertex(x, j, ref tempVectors[0]);
GetVertex(x + 1, j + 1, ref tempVectors[1]);
GetVertex(x, j + 1, ref tempVectors[2]);
vertices.Clear();
for (int a = 0; a < tempVectors.Length; ++a)
{
vertices.Add(tempVectors[a]);
}
callback.ProcessTriangle(vertices, x, j);
}
else
{
//first triangle
GetVertex(x, j, ref tempVectors[0]);
GetVertex(x, j + 1, ref tempVectors[1]);
GetVertex(x + 1, j, ref tempVectors[2]);
vertices.Clear();
for (int a = 0; a < tempVectors.Length; ++a)
{
vertices.Add(tempVectors[a]);
}
callback.ProcessTriangle(vertices, x, j);
//second triangle
GetVertex(x + 1, j, ref tempVectors[0]);
GetVertex(x, j + 1, ref tempVectors[1]);
GetVertex(x + 1, j + 1, ref tempVectors[2]);
vertices.Clear();
for (int a = 0; a < tempVectors.Length; ++a)
{
vertices.Add(tempVectors[a]);
}
callback.ProcessTriangle(vertices, x, j);
}
}
}
}
public static void Split(ObjectArray<DbvtNode> leaves, ObjectArray<DbvtNode> left, ObjectArray<DbvtNode> right, ref IndexedVector3 org, ref IndexedVector3 axis)
{
left.Resize(0);
right.Resize(0);
for (int i = 0, ni = leaves.Count; i < ni; ++i)
{
if (IndexedVector3.Dot(axis, leaves[i].volume.Center() - org) < 0)
{
left.Add(leaves[i]);
}
else
{
right.Add(leaves[i]);
}
}
}
// Portable static method: prerequisite call: m_dynamicsWorld.getBroadphase().getOverlappingPairCache().setInternalGhostPairCallback(new btGhostPairCallback());
public static void GetCollidingObjectsInsidePairCachingGhostObject(DiscreteDynamicsWorld m_dynamicsWorld, PairCachingGhostObject m_pairCachingGhostObject, ObjectArray<CollisionObject> collisionArrayOut)
{
bool addOnlyObjectsWithNegativeDistance = true; // With "false" things don't change much, and the code is a bit faster and cleaner...
collisionArrayOut.Resize(0);
if (m_pairCachingGhostObject == null || m_dynamicsWorld == null) return;
//#define USE_PLAIN_COLLISION_WORLD // We dispatch all collision pairs of the ghost object every step (slow)
#if USE_PLAIN_COLLISION_WORLD
//======================================================================================================
// I thought this line was no longer needed, but it seems to be necessary (and I believe it's an expensive call):
m_dynamicsWorld.getDispatcher().dispatchAllCollisionPairs(m_pairCachingGhostObject.getOverlappingPairCache(), m_dynamicsWorld.getDispatchInfo(), m_dynamicsWorld.getDispatcher());
// Maybe the call can be automatically triggered by some other Bullet call (I'm almost sure I could comment it out in another demo I made long ago...)
// So by now the general rule is: in real projects, simply comment it out and see if it works!
//======================================================================================================
// UPDATE: in dynamic worlds, the line above can be commented out and the broadphase pair can be retrieved through the call to findPair(...) below.
// In collision worlds probably the above line is needed only if collision detection for all the bodies hasn't been made... This is something
// I'm still not sure of... the general rule is to try to comment out the line above and try to use findPair(...) and see if it works whenever possible....
//======================================================================================================
#endif //USE_PLAIN_COLLISION_WORLD
ObjectArray<BroadphasePair> collisionPairs = m_pairCachingGhostObject.GetOverlappingPairCache().GetOverlappingPairArray();
int numObjects = collisionPairs.Count;
PersistentManifoldArray m_manifoldArray = new PersistentManifoldArray();
bool added;
for (int i = 0; i < numObjects; i++)
{
m_manifoldArray.Resize(0);
#if USE_PLAIN_COLLISION_WORLD
const btBroadphasePair& collisionPair = collisionPairs[i];
if (collisionPair.m_algorithm) collisionPair.m_algorithm.getAllContactManifolds(m_manifoldArray);
else { // THIS SHOULD NEVER HAPPEN, AND IF IT DOES, PLEASE RE-ENABLE the "call" a few lines above...
printf("No collisionPair.m_algorithm - probably m_dynamicsWorld.getDispatcher().dispatchAllCollisionPairs(...) must be missing.\n");
}
#else // USE_PLAIN_COLLISION_WORLD
BroadphasePair cPair = collisionPairs[i];
//unless we manually perform collision detection on this pair, the contacts are in the dynamics world paircache:
BroadphasePair collisionPair = m_dynamicsWorld.GetPairCache().FindPair(cPair.m_pProxy0, cPair.m_pProxy1);
if (collisionPair == null)
{
continue;
}
if (collisionPair.m_algorithm != null)
{
collisionPair.m_algorithm.GetAllContactManifolds(m_manifoldArray);
}
else
{ // THIS SHOULD NEVER HAPPEN, AND IF IT DOES, PLEASE RE-ENABLE the "call" a few lines above...
//printf("No collisionPair.m_algorithm - probably m_dynamicsWorld.getDispatcher().dispatchAllCollisionPairs(...) must be missing.\n");
}
#endif //USE_PLAIN_COLLISION_WORLD
added = false;
for (int j = 0; j < m_manifoldArray.Count; j++)
{
PersistentManifold manifold = m_manifoldArray[j];
// Here we are in the narrowphase, but can happen that manifold.getNumContacts()==0:
if (addOnlyObjectsWithNegativeDistance)
{
for (int p = 0, numContacts = manifold.GetNumContacts(); p < numContacts; p++)
{
ManifoldPoint pt = manifold.GetContactPoint(p);
if (pt.GetDistance() < 0.0)
{
// How can I be sure that the colObjs are all distinct ? I use the "added" flag.
collisionArrayOut.Add((CollisionObject)(manifold.GetBody0() == m_pairCachingGhostObject ? manifold.GetBody1() : manifold.GetBody0()));
added = true;
break;
}
}
if (added)
{
break;
}
}
else if (manifold.GetNumContacts() > 0)
{
collisionArrayOut.Add((CollisionObject)(manifold.GetBody0() == m_pairCachingGhostObject ? manifold.GetBody1() : manifold.GetBody0()));
break;
}
}
}
}
public CollisionShape BuildCorner()
{
// slope.
IndexedVector3[] vertices = new IndexedVector3[]{new IndexedVector3(0,0,0),new IndexedVector3(1,0,0),new IndexedVector3(0,0,1),new IndexedVector3(1,0,1),
new IndexedVector3(0,1,0),new IndexedVector3(1,1,0),new IndexedVector3(0,1,1),new IndexedVector3(1,1,1)};
//int[] indices = new int[] { 0, 4, 5, 4, 6, 7, 7, 5, 4, 0, 4, 6, 6, 2, 0, 2, 6, 7, 4,5,0,2,2,7,5};
//int[] indices = new int[] { 0, 4, 5, 4, 6, 7, 7, 5, 4, 6, 4,0,0,2,6, 7, 6, 2, 4, 5, 0, 2, 2, 7, 5 };
int[] indices = new int[] { 1,4,5,
1,5,7,
7,3,1,
3,7,6,
7,5,4,
4,6,7,
4,1,3,
3,6,4
};
int vertStride = 1;
int indexStride = 3;
ObjectArray<IndexedVector3> vertexArray = new ObjectArray<IndexedVector3>();
for (int i = 0; i < vertices.Length; ++i)
{
vertexArray.Add(vertices[i]);
}
ObjectArray<int> intArray = new ObjectArray<int>();
for (int i = 0; i < indices.Length; ++i)
{
intArray.Add(indices[i]);
}
TriangleIndexVertexArray indexVertexArray = new TriangleIndexVertexArray(indices.Length/3, intArray, indexStride, vertexArray.Count, vertexArray, vertStride);
TriangleMeshShape triangleMesh = new TriangleMeshShape(indexVertexArray);
//TriangleMeshShape triangleMesh = new BvhTriangleMeshShape(indexVertexArray,true,true);
return triangleMesh;
}
public void BuildAndProcessIslands(IDispatcher dispatcher, CollisionWorld collisionWorld, IIslandCallback callback)
{
ObjectArray<CollisionObject> collisionObjects = collisionWorld.GetCollisionObjectArray();
BuildIslands(dispatcher,collisionWorld);
int endIslandIndex=1;
int startIslandIndex;
int numElem = GetUnionFind().GetNumElements();
//BT_PROFILE("processIslands");
if(!m_splitIslands)
{
ObjectArray<PersistentManifold> manifolds = dispatcher.GetInternalManifoldPointer();
int maxNumManifolds = dispatcher.GetNumManifolds();
callback.ProcessIsland(collisionObjects,collisionObjects.Count,manifolds,maxNumManifolds, -1);
}
else
{
// Sort manifolds, based on islands
// Sort the vector using predicate and std::sort
//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
int numManifolds = m_islandmanifold.Count;
if (numManifolds != 0 && m_islandmanifold[0].GetNumContacts() > 0)
{
int ibreak = 0;
}
//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
//m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
//((ObjectArray<PersistentManifold>)m_islandmanifold).Sort();
m_islandmanifold.Sort(new Comparison<PersistentManifold>(PersistentManifoldSortPredicate));
//now process all active islands (sets of manifolds for now)
int startManifoldIndex = 0;
int endManifoldIndex = 1;
//int islandId;
// printf("Start Islands\n");
//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
{
int islandId = GetUnionFind().GetElement(startIslandIndex).m_id;
if (BulletGlobals.g_streamWriter != null && debugIslands)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("buildAndProcessIslands start[{0}] end[{1}] id[{2}]", startIslandIndex, endIslandIndex, islandId));
}
bool islandSleeping = false;
for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (GetUnionFind().GetElement(endIslandIndex).m_id == islandId);endIslandIndex++)
{
int i = GetUnionFind().GetElement(endIslandIndex).m_sz;
CollisionObject colObj0 = collisionObjects[i];
m_islandBodies.Add(colObj0);
if (!colObj0.IsActive())
{
islandSleeping = true;
//islandSleeping = false;
}
}
//find the accompanying contact manifold for this islandId
int numIslandManifolds = 0;
PersistentManifold startManifold = null;
if (startManifoldIndex<numManifolds)
{
int curIslandId = GetIslandId(m_islandmanifold[startManifoldIndex]);
if (curIslandId == islandId)
{
startManifold = m_islandmanifold[startManifoldIndex];
for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == GetIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
{
}
/// Process the actual simulation, only if not sleeping/deactivated
numIslandManifolds = endManifoldIndex-startManifoldIndex;
}
}
if (!islandSleeping)
{
// pass shortedned list to callback.
ObjectArray<PersistentManifold> subList = new ObjectArray<PersistentManifold>();
for(int i=0;i<numIslandManifolds;++i)
{
subList.Add(m_islandmanifold[startManifoldIndex+i]);
}
callback.ProcessIsland(m_islandBodies,m_islandBodies.Count,subList,numIslandManifolds, islandId);
// printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
}
if (numIslandManifolds != 0)
{
startManifoldIndex = endManifoldIndex;
}
m_islandBodies.Clear();
}
} // else if(!splitIslands)
}
public void QueryNode(QuadTreeNode node, ObjectArray<QuadTreeNode> results, IndexedVector3 source,IndexedVector3 direction)
{
//if(node.children == null && node.Intersects(raySource,rayTarget))
// add the lowest level.
if (node.children == null)
{
results.Add(node);
#if DEBUG_ACCELERATOR
if (BulletGlobals.gDebugDraw != null)
{
IndexedVector3 drawMin = new IndexedVector3(node.boundingBox.Min);
IndexedVector3 drawMax = new IndexedVector3(node.boundingBox.Max);
IndexedVector3 worldMin = LocalToWorld2(drawMin);
IndexedVector3 worldMax = LocalToWorld2(drawMax);
BulletGlobals.gDebugDraw.DrawAabb(worldMin, worldMax, new IndexedVector3(1, 1, 1));
}
#endif
}
else
{
// simple rescursive for now.
for (int i = 0; i < 4; ++i)
{
if (node.children[i].Intersects(source,direction))
{
QueryNode(node.children[i], results, source,direction);
}
}
}
}
CollisionShape BuildLargeMesh()
{
//int vertStride = sizeof(IndexedVector3);
//int indexStride = 3*sizeof(int);
int vertStride = 1;
int indexStride = 3;
ObjectArray<IndexedVector3> vertexArray = new ObjectArray<IndexedVector3>();
for (int i = 0; i < vertices.Length; ++i)
{
vertexArray.Add(vertices[i]);
}
ObjectArray<int> intArray = new ObjectArray<int>();
for (int i = 0; i < indices.Length; ++i)
{
intArray.Add(indices[i]);
}
//TriangleIndexVertexArray indexVertexArray = new TriangleIndexVertexArray(DemoMeshes.BUNNY_NUM_TRIANGLES, DemoMeshes.gBunnyIndices, 3, DemoMeshes.BUNNY_NUM_VERTICES, DemoMeshes.gBunnyVertices, 3);
TriangleIndexVertexArray indexVertexArray = new TriangleIndexVertexArray(numTriangles, intArray, indexStride, vertexArray.Count, vertexArray, vertStride);
TriangleMeshShape triangleMesh = new TriangleMeshShape(indexVertexArray);
//TriangleMeshShape triangleMesh = new BvhTriangleMeshShape(indexVertexArray,true,true);
return triangleMesh;
}
protected virtual void SolveConstraints(ContactSolverInfo solverInfo)
{
//sorted version of all btTypedConstraint, based on islandId
ObjectArray<TypedConstraint> sortedConstraints = new ObjectArray<TypedConstraint>(GetNumConstraints());
if (BulletGlobals.g_streamWriter != null && debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("solveConstraints");
}
for (int i = 0; i < GetNumConstraints(); i++)
{
sortedConstraints.Add(m_constraints[i]);
}
// btAssert(0);
//sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
sortedConstraints.Sort(new SortConstraintOnIslandPredicate());
ObjectArray<TypedConstraint> constraintsPtr = GetNumConstraints() > 0 ? sortedConstraints : null;
InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, m_constraintSolver, constraintsPtr, sortedConstraints.Count, m_debugDrawer, m_dispatcher1);
if (BulletGlobals.g_streamWriter != null && debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("prepareSolve");
}
m_constraintSolver.PrepareSolve(GetCollisionWorld().GetNumCollisionObjects(), GetCollisionWorld().GetDispatcher().GetNumManifolds());
if (BulletGlobals.g_streamWriter != null && debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("buildAndProcessIsland");
}
/// solve all the constraints for this island
m_islandManager.BuildAndProcessIslands(GetCollisionWorld().GetDispatcher(), GetCollisionWorld(), solverCallback);
solverCallback.ProcessConstraints();
m_constraintSolver.AllSolved(solverInfo, m_debugDrawer);
}
protected void GImpactVsShapeFindPairs(
ref IndexedMatrix trans0,
ref IndexedMatrix trans1,
GImpactShapeInterface shape0,
CollisionShape shape1,
ObjectArray<int> collided_primitives)
{
AABB boxshape = new AABB();
if (shape0.HasBoxSet())
{
IndexedMatrix trans1to0 = trans0.Inverse();
//trans1to0 *= trans1;
trans1to0 = trans1to0 * trans1;
//trans1to0 = MathUtil.BulletMatrixMultiply(trans1,trans1to0);
shape1.GetAabb(ref trans1to0, out boxshape.m_min, out boxshape.m_max);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "GImpactAglo::GImpactVsShapeFindPairs trans1to0", trans1to0);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box min", boxshape.m_min);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box max", boxshape.m_max);
}
shape0.GetBoxSet().BoxQuery(ref boxshape, collided_primitives);
}
else
{
shape1.GetAabb(ref trans1, out boxshape.m_min, out boxshape.m_max);
AABB boxshape0 = new AABB();
int i = shape0.GetNumChildShapes();
while (i-- != 0)
{
shape0.GetChildAabb(i, ref trans0, out boxshape0.m_min, out boxshape0.m_max);
if (boxshape.HasCollision(ref boxshape0))
{
collided_primitives.Add(i);
}
}
}
}
public virtual void InternalProcessAllTriangles(IInternalTriangleIndexCallback callback,ref Vector3 aabbMin,ref Vector3 aabbMax)
{
int numtotalphysicsverts = 0;
int part,graphicssubparts = GetNumSubParts();
Object vertexbase = null;
Object indexbase = null;
int indexstride = 3;
PHY_ScalarType type = PHY_ScalarType.PHY_FLOAT;
PHY_ScalarType gfxindextype = PHY_ScalarType.PHY_INTEGER;
int stride = 0,numverts = 0 ,numtriangles = 0;
ObjectArray<Vector3> triangle = new ObjectArray<Vector3>(3);
// ugly. - could do with either a static , or allowing these methods to take a list or a fixed array...
triangle.Add(Vector3.Zero);
triangle.Add(Vector3.Zero);
triangle.Add(Vector3.Zero);
float graphicsBase = 0f;
Vector3 meshScaling = GetScaling();
///if the number of parts is big, the performance might drop due to the innerloop switch on indextype
for (part=0;part<graphicssubparts ;part++)
{
getLockedReadOnlyVertexIndexBase(out vertexbase,out numverts,out type,out stride,out indexbase,out indexstride,out numtriangles,out gfxindextype,part);
numtotalphysicsverts+=numtriangles*3; //upper bound
switch (gfxindextype)
{
case PHY_ScalarType.PHY_INTEGER:
{
ObjectArray<int> indexList = (ObjectArray<int>)indexbase;
//ObjectArray<float> vertexList = (ObjectArray<float>)vertexbase;
// hack for now - need to tidy this..
if (vertexbase is ObjectArray<Vector3>)
{
ObjectArray<Vector3> vertexList = (ObjectArray<Vector3>)vertexbase;
for (int gfxindex = 0; gfxindex < numtriangles; gfxindex++)
{
// FIXME - Work ref the properindexing on this.
int index = gfxindex * indexstride;
int triIndex = (gfxindex * indexstride);
triangle[0] = vertexList[indexList[triIndex]] * meshScaling;
triangle[1] = vertexList[indexList[triIndex+1]] * meshScaling;
triangle[2] = vertexList[indexList[triIndex+2]] * meshScaling;
callback.InternalProcessTriangleIndex(triangle, part, gfxindex);
}
}
else if (vertexbase is ObjectArray<float>)
{
//triangle[0] = new Vector3(vertexList[indexList[triIndex]] * meshScaling.X, vertexList[indexList[triIndex + 1]] * meshScaling.Y, vertexList[indexList[triIndex + 2]] * meshScaling.Z);
//triangle[1] = new Vector3(vertexList[indexList[triIndex]] * meshScaling.X, vertexList[indexList[triIndex + 1]] * meshScaling.Y, vertexList[indexList[triIndex + 2]] * meshScaling.Z);
//triangle[2] = new Vector3(vertexList[indexList[triIndex]] * meshScaling.X, vertexList[indexList[triIndex + 1]] * meshScaling.Y, vertexList[indexList[triIndex + 2]] * meshScaling.Z);
ObjectArray<float> vertexList = (ObjectArray<float>)vertexbase;
for (int gfxindex = 0; gfxindex < numtriangles; gfxindex++)
{
// FIXME - Work ref the properindexing on this.
int index = gfxindex * indexstride;
int triIndex = (gfxindex * indexstride);
// ugly!!
triangle[0] = new Vector3(vertexList[indexList[triIndex]], vertexList[indexList[triIndex] + 1], vertexList[indexList[triIndex] + 2]) * meshScaling;
triangle[1] = new Vector3(vertexList[indexList[triIndex+1]], vertexList[indexList[triIndex+1] + 1], vertexList[indexList[triIndex+1] + 2]) * meshScaling;
triangle[2] = new Vector3(vertexList[indexList[triIndex+2]], vertexList[indexList[triIndex+2] + 1], vertexList[indexList[triIndex+2] + 2]) * meshScaling;
callback.InternalProcessTriangleIndex(triangle, part, gfxindex);
}
}
else
{
Debug.Assert(false); // unsupported type ....
}
break;
}
case PHY_ScalarType.PHY_SHORT:
{
ObjectArray<ushort> indexList = (ObjectArray<ushort>)indexbase;
if (vertexbase is ObjectArray<Vector3>)
{
ObjectArray<Vector3> vertexList = (ObjectArray<Vector3>)vertexbase;
for (int gfxindex = 0; gfxindex < numtriangles; gfxindex++)
{
// FIXME - Work ref the properindexing on this.
int index = gfxindex * indexstride;
int triIndex = (gfxindex * indexstride);
triangle[0] = vertexList[indexList[triIndex]] * meshScaling;
triangle[1] = vertexList[indexList[triIndex + 1]] * meshScaling;
triangle[2] = vertexList[indexList[triIndex + 2]] * meshScaling;
callback.InternalProcessTriangleIndex(triangle, part, gfxindex);
}
}
else if (vertexbase is ObjectArray<float>)
{
//triangle[0] = new Vector3(vertexList[indexList[triIndex]] * meshScaling.X, vertexList[indexList[triIndex + 1]] * meshScaling.Y, vertexList[indexList[triIndex + 2]] * meshScaling.Z);
//triangle[1] = new Vector3(vertexList[indexList[triIndex]] * meshScaling.X, vertexList[indexList[triIndex + 1]] * meshScaling.Y, vertexList[indexList[triIndex + 2]] * meshScaling.Z);
//triangle[2] = new Vector3(vertexList[indexList[triIndex]] * meshScaling.X, vertexList[indexList[triIndex + 1]] * meshScaling.Y, vertexList[indexList[triIndex + 2]] * meshScaling.Z);
ObjectArray<float> vertexList = (ObjectArray<float>)vertexbase;
for (int gfxindex = 0; gfxindex < numtriangles; gfxindex++)
{
// FIXME - Work ref the properindexing on this.
int index = gfxindex * indexstride;
int triIndex = (gfxindex * indexstride);
// ugly!!
triangle[0] = new Vector3(vertexList[indexList[triIndex]], vertexList[indexList[triIndex] + 1], vertexList[indexList[triIndex] + 2]) * meshScaling;
triangle[1] = new Vector3(vertexList[indexList[triIndex + 1]], vertexList[indexList[triIndex + 1] + 1], vertexList[indexList[triIndex + 1] + 2]) * meshScaling;
triangle[2] = new Vector3(vertexList[indexList[triIndex + 2]], vertexList[indexList[triIndex + 2] + 1], vertexList[indexList[triIndex + 2] + 2]) * meshScaling;
callback.InternalProcessTriangleIndex(triangle, part, gfxindex);
}
}
else
{
Debug.Assert(false); // unsupported type ....
}
break;
}
default:
{
Debug.Assert((gfxindextype == PHY_ScalarType.PHY_INTEGER) || (gfxindextype == PHY_ScalarType.PHY_SHORT));
break;
}
}
UnLockReadOnlyVertexBase(part);
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
m_ownManifold = true;
}
//resultOut = new ManifoldResult();
resultOut.SetPersistentManifold(m_manifoldPtr);
//comment-out next line to test multi-contact generation
//resultOut.GetPersistentManifold().ClearManifold();
ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;
IndexedVector3 normalOnB;
IndexedVector3 pointOnBWorld;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if ((min0.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = min0 as CapsuleShape;
CapsuleShape capsuleB = min1 as CapsuleShape;
//IndexedVector3 localScalingA = capsuleA.GetLocalScaling();
//IndexedVector3 localScalingB = capsuleB.GetLocalScaling();
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float dist = CapsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.GetHalfHeight(), capsuleA.GetRadius(),
capsuleB.GetHalfHeight(), capsuleB.GetRadius(), capsuleA.GetUpAxis(), capsuleB.GetUpAxis(),
body0.GetWorldTransform(), body1.GetWorldTransform(), threshold);
if (dist < threshold)
{
Debug.Assert(normalOnB.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON));
resultOut.AddContactPoint(ref normalOnB, ref pointOnBWorld, dist);
}
resultOut.RefreshContactPoints();
return;
}
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(), body1.getWorldTransform());
}
if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance() <= 0.f)
#endif //USE_SEPDISTANCE_UTIL2
{
ClosestPointInput input = ClosestPointInput.Default();
using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
{
gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.SetMinkowskiA(min0);
gjkPairDetector.SetMinkowskiB(min1);
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
input.m_maximumDistanceSquared = float.MaxValue;
}
else
#endif //USE_SEPDISTANCE_UTIL2
{
input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
//input.m_stackAlloc = dispatchInfo.m_stackAllocator;
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
if (min0.IsPolyhedral() && min1.IsPolyhedral())
{
DummyResult dummy = new DummyResult();
PolyhedralConvexShape polyhedronA = min0 as PolyhedralConvexShape;
PolyhedralConvexShape polyhedronB = min1 as PolyhedralConvexShape;
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetConvexPolyhedron() != null)
{
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float minDist = float.MinValue;
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);
bool foundSepAxis = true;
if (dispatchInfo.m_enableSatConvex)
{
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
#if ZERO_MARGIN
foundSepAxis = true; //gjkPairDetector.getCachedSeparatingDistance()<0.f;
#else
foundSepAxis = gjkPairDetector.GetCachedSeparatingDistance() < (min0.GetMargin() + min1.GetMargin());
#endif
}
}
if (foundSepAxis)
{
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
PolyhedralContactClipping.ClipHullAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(), minDist - threshold, threshold, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
else
{
//we can also deal with convex versus triangle (without connectivity data)
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
{
m_vertices.Clear();
TriangleShape tri = polyhedronB as TriangleShape;
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[0]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[1]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[2]);
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);;
float minDist = float.MinValue;
float maxDist = threshold;
bool foundSepAxis = false;
if (false)
{
polyhedronB.InitializePolyhedralFeatures();
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = gjkPairDetector.getCachedSeparatingDistance();
//maxDist = threshold;
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
foundSepAxis = true;
}
}
if (foundSepAxis)
{
PolyhedralContactClipping.ClipFaceAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(),
body0.GetWorldTransform(), m_vertices, minDist - threshold, maxDist, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
}
}
gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if USE_SEPDISTANCE_UTIL2
float sepDist = 0.f;
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
sepDist = gjkPairDetector.getCachedSeparatingDistance();
if (sepDist > MathUtil.SIMD_EPSILON)
{
sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
//now perturbe directions to get multiple contact points
}
}
#endif //USE_SEPDISTANCE_UTIL2
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
if (m_numPerturbationIterations > 0 && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
{
IndexedVector3 v0, v1;
IndexedVector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
sepNormalWorldSpace.Normalize();
TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1);
bool perturbeA = true;
const float angleLimit = 0.125f * MathUtil.SIMD_PI;
float perturbeAngle;
float radiusA = min0.GetAngularMotionDisc();
float radiusB = min1.GetAngularMotionDisc();
if (radiusA < radiusB)
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusA;
perturbeA = true;
}
else
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusB;
perturbeA = false;
}
if (perturbeAngle > angleLimit)
{
perturbeAngle = angleLimit;
}
IndexedMatrix unPerturbedTransform;
if (perturbeA)
{
unPerturbedTransform = input.m_transformA;
}
else
{
unPerturbedTransform = input.m_transformB;
}
for (int i = 0; i < m_numPerturbationIterations; i++)
{
if (v0.LengthSquared() > MathUtil.SIMD_EPSILON)
{
IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle);
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
IndexedQuaternion rotq = new IndexedQuaternion(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
input.m_transformA._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body0.GetWorldTransform()._basis);
input.m_transformB = body1.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformA, 10.0f);
#endif //DEBUG_CONTACTS
}
else
{
input.m_transformA = body0.GetWorldTransform();
input.m_transformB._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body1.GetWorldTransform()._basis);
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformB, 10.0f);
#endif
}
PerturbedContactResult perturbedResultOut = new PerturbedContactResult(resultOut, ref input.m_transformA, ref input.m_transformB, ref unPerturbedTransform, perturbeA, dispatchInfo.getDebugDraw());
gjkPairDetector.GetClosestPoints(ref input, perturbedResultOut, dispatchInfo.getDebugDraw(), false);
}
}
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > MathUtil.SIMD_EPSILON))
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(), sepDist, body0.getWorldTransform(), body1.getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2
}
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
///the clipFace method is used internally
public static void ClipFace(ObjectArray<IndexedVector3> pVtxIn, ObjectArray<IndexedVector3> ppVtxOut, ref IndexedVector3 planeNormalWS, float planeEqWS)
{
int ve;
float ds, de;
int numVerts = pVtxIn.Count;
if (numVerts < 2)
return;
IndexedVector3 firstVertex = pVtxIn[pVtxIn.Count - 1];
IndexedVector3 endVertex = pVtxIn[0];
ds = IndexedVector3.Dot(ref planeNormalWS, ref firstVertex);
ds += planeEqWS;
for (ve = 0; ve < numVerts; ve++)
{
endVertex = pVtxIn[ve];
de = IndexedVector3.Dot(ref planeNormalWS, ref endVertex);
de += planeEqWS;
if (ds < 0)
{
if (de < 0)
{
// Start < 0, end < 0, so output endVertex
ppVtxOut.Add(endVertex);
}
else
{
// Start < 0, end >= 0, so output intersection
ppVtxOut.Add(MathUtil.Vector3Lerp(ref firstVertex, ref endVertex, (float)(ds * 1.0f / (ds - de))));
}
}
else
{
if (de < 0)
{
// Start >= 0, end < 0 so output intersection and end
ppVtxOut.Add(MathUtil.Vector3Lerp(ref firstVertex, ref endVertex, (float)(ds * 1.0f / (ds - de))));
ppVtxOut.Add(endVertex);
}
}
firstVertex = endVertex;
ds = de;
}
}
public static void ClipHullAgainstHull(ref IndexedVector3 separatingNormal1, ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
IndexedVector3 separatingNormal = separatingNormal1.Normalized();
IndexedVector3 c0 = transA * hullA.m_localCenter;
IndexedVector3 c1 = transB * hullB.m_localCenter;
IndexedVector3 DeltaC2 = c0 - c1;
float curMaxDist = maxDist;
int closestFaceB = -1;
float dmax = float.MinValue;
{
for (int face = 0; face < hullB.m_faces.Count; face++)
{
IndexedVector3 Normal = new IndexedVector3(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
IndexedVector3 WorldNormal = transB._basis * Normal;
float d = IndexedVector3.Dot(WorldNormal, separatingNormal);
if (d > dmax)
{
dmax = d;
closestFaceB = face;
}
}
}
// setup initial clip face (minimizing face from hull B)
ObjectArray<IndexedVector3> worldVertsB1 = new ObjectArray<IndexedVector3>();
{
Face polyB = hullB.m_faces[closestFaceB];
int numVertices = polyB.m_indices.Count;
for (int e0 = 0; e0 < numVertices; e0++)
{
IndexedVector3 b = hullB.m_vertices[polyB.m_indices[e0]];
// check this to see if it is transposed version
worldVertsB1.Add(transB * b);
}
}
if (closestFaceB >= 0)
{
ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut);
}
}
//InplaceSolverIslandCallback operator=(InplaceSolverIslandCallback& other)
//{
// Debug.Assert(false);
// //(void)other;
// return *this;
//}
public virtual void ProcessIsland(ObjectArray<CollisionObject> bodies, int numBodies, ObjectArray<PersistentManifold> manifolds, int numManifolds, int islandId)
{
if (islandId < 0)
{
if (numManifolds + m_numConstraints > 0)
{
///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
m_solver.SolveGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, m_solverInfo, m_debugDrawer, m_dispatcher);
}
}
else
{
//also add all non-contact constraints/joints for this island
ObjectArray<TypedConstraint> startConstraint = new ObjectArray<TypedConstraint>();
int numCurConstraints = 0;
int i = 0;
//find the first constraint for this island
for (i = 0; i < m_numConstraints; i++)
{
if (DiscreteDynamicsWorld.GetConstraintIslandId(m_sortedConstraints[i]) == islandId)
{
// FIXME - Do we need add everything after i to mirror the pointer?
for (int k = i; k < m_numConstraints; ++k)
{
startConstraint.Add(m_sortedConstraints[k]);
}
break;
}
}
//count the number of constraints in this island
for (; i < m_numConstraints; i++)
{
if (DiscreteDynamicsWorld.GetConstraintIslandId(m_sortedConstraints[i]) == islandId)
{
numCurConstraints++;
}
}
if (m_solverInfo.m_minimumSolverBatchSize <= 1)
{
///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
if (numManifolds + numCurConstraints != 0)
{
m_solver.SolveGroup(bodies, numBodies, manifolds, numManifolds, startConstraint, numCurConstraints, m_solverInfo, m_debugDrawer, m_dispatcher);
}
}
else
{
for (i = 0; i < numBodies; i++)
{
m_bodies.Add(bodies[i]);
}
for (i = 0; i < numManifolds; i++)
{
m_manifolds.Add(manifolds[i]);
}
for (i = 0; i < numCurConstraints; i++)
{
m_constraints.Add(startConstraint[i]);
}
if ((m_constraints.Count + m_manifolds.Count) > m_solverInfo.m_minimumSolverBatchSize)
{
ProcessConstraints();
}
else
{
//printf("deferred\n");
}
}
// ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
// if (numManifolds + numCurConstraints > 0)
// {
////solveGroup(ObjectArray<CollisionObject> bodies,int numBodies,ObjectArray<PersistentManifold> manifold,int numManifolds,ObjectArray<TypedConstraint> constraints,int numConstraints, ContactSolverInfo info,IDebugDraw debugDrawer, IDispatcher dispatcher);
// m_solver.solveGroup(bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_dispatcher);
// }
}
}
