public void UpdateCollision()
{
if (geometry.CanRender)
{
Body[] bodies = PhysicsHelper.PhysicsBodiesVolume(boundsWorldSpaceCollision);
if(CollisionMesh == null && bodies.Length > 0)
{
GenerateCollisionMesh();
CollisionDeleteTime = CollisionDeleteTimeS;
}
else if (CollisionMesh != null)
{
if (bodies.Length < 1)
{
CollisionDeleteTime -= Time.GameTime.ElapsedTime;
if (CollisionDeleteTime <= 0)
{
PhysicsSystem.CurrentPhysicsSystem.CollisionSystem.RemoveCollisionSkin(Collision);
Collision = null;
CollisionMesh = null;
}
}
else
{
CollisionDeleteTime = CollisionDeleteTimeS;
}
}
}
}
public override Primitive Clone()
{
TriangleMesh triangleMesh = new TriangleMesh();
// triangleMesh.CreateMesh(vertices, triangleVertexIndices, maxTrianglesPerCell, minCellSize);
// its okay to share the octree
triangleMesh.octree = this.octree;
triangleMesh.Transform = Transform;
return triangleMesh;
}
public TriangleMeshObject(Game game, Model model, Matrix orientation, Vector3 position)
: base(game, model)
{
body = new Body();
collision = new CollisionSkin(null);
triangleMesh = new TriangleMesh();
List<Vector3> vertexList = new List<Vector3>();
List<TriangleVertexIndices> indexList = new List<TriangleVertexIndices>();
ExtractData(vertexList, indexList, model);
triangleMesh.CreateMesh(vertexList,indexList, 4, 1.0f);
collision.AddPrimitive(triangleMesh,new MaterialProperties(0.8f,0.7f,0.6f));
PhysicsSystem.CurrentPhysicsSystem.CollisionSystem.AddCollisionSkin(collision);
}
// Sets the model being simulated, extracts vertices, etc.
public void SetModel(string ModelName)
{
Model Model = Engine.Content.Load<Model>(ModelName);
this.modelName = ModelName;
CollisionSkin.RemoveAllPrimitives();
triangleMesh = new TriangleMesh();
List<Vector3> vertexList = new List<Vector3>();
List<TriangleVertexIndices> indexList =
new List<TriangleVertexIndices>();
ExtractData(vertexList, indexList, Model);
triangleMesh.CreateMesh(vertexList, indexList, 4, 1.0f);
CollisionSkin.AddPrimitive(triangleMesh,
new MaterialProperties(0.8f, 0.7f, 0.6f));
Mass = Mass;
}
public TriangleMeshObject(Game game, Model model, Matrix orientation, Vector3 position)
: base(game, model)
{
body = new Body();
collision = new CollisionSkin(null);
triangleMesh = new TriangleMesh();
List<Vector3> vertexList = new List<Vector3>();
List<TriangleVertexIndices> indexList = new List<TriangleVertexIndices>();
ExtractData(vertexList, indexList, model);
triangleMesh.CreateMesh(vertexList,indexList, 4, 1.0f);
collision.AddPrimitive(triangleMesh,new MaterialProperties(0.8f,0.7f,0.6f));
PhysicsSystem.CurrentPhysicsSystem.CollisionSystem.AddCollisionSkin(collision);
// Transform
collision.ApplyLocalTransform(new JigLibX.Math.Transform(position, orientation));
// we also need to move this dummy, so the object is *rendered* at the correct positiob
body.MoveTo(position, orientation);
}
/// <summary>
/// CollDetectCapsuleStaticMeshOverlap
/// </summary>
/// <param name="oldCapsule"></param>
/// <param name="newCapsule"></param>
/// <param name="mesh"></param>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
private void CollDetectCapsuleStaticMeshOverlap(Capsule oldCapsule, Capsule newCapsule,
TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
float capsuleTolR = collTolerance + newCapsule.Radius;
float capsuleTolR2 = capsuleTolR * capsuleTolR;
Vector3 collNormal = Vector3.Zero;
BoundingBox bb = BoundingBoxHelper.InitialBox;
BoundingBoxHelper.AddCapsule(newCapsule, ref bb);
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo* collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
int* potentialTriangles = stackalloc int[MaxLocalStackTris];
{
{
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed (SmallCollPointInfo* collPts = collPtArray)
{
int[] potTriArray = IntStackAlloc();
fixed (int* potentialTriangles = potTriArray)
{
#endif
int numCollPts = 0;
int numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb);
Vector3 capsuleStart = newCapsule.Position;
Vector3 capsuleEnd = newCapsule.GetEnd();
Matrix4 meshInvTransform = mesh.InverseTransformMatrix;
Vector3 meshSpaceCapsuleStart = Vector3.Transform(capsuleStart, meshInvTransform);
Vector3 meshSpaceCapsuleEnd = Vector3.Transform(capsuleEnd, meshInvTransform);
for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle)
{
IndexedTriangle meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]);
// we do the plane test using the capsule in mesh space
float distToStart = meshTriangle.Plane.DotCoordinate(meshSpaceCapsuleStart);
float distToEnd = meshTriangle.Plane.DotCoordinate(meshSpaceCapsuleEnd);
// BEN-BUG-FIX: Fixed by replacing 0.0F with -capsuleTolR.
if ((distToStart > capsuleTolR && distToEnd > capsuleTolR)
|| (distToStart < -capsuleTolR && distToEnd < -capsuleTolR))
continue;
// we now transform the triangle into world space (we could keep leave the mesh alone
// but at this point 3 vector transforms is probably not a major slow down)
int i0, i1, i2;
meshTriangle.GetVertexIndices(out i0, out i1, out i2);
Vector3 triVec0;
Vector3 triVec1;
Vector3 triVec2;
mesh.GetVertex(i0, out triVec0);
mesh.GetVertex(i1, out triVec1);
mesh.GetVertex(i2, out triVec2);
// Deano move tri into world space
Matrix4 transformMatrix = mesh.TransformMatrix;
Vector3.Transform(ref triVec0, ref transformMatrix, out triVec0);
Vector3.Transform(ref triVec1, ref transformMatrix, out triVec1);
Vector3.Transform(ref triVec2, ref transformMatrix, out triVec2);
Triangle triangle = new Triangle(ref triVec0, ref triVec1, ref triVec2);
Segment seg = new Segment(capsuleStart, capsuleEnd - capsuleStart);
float tS, tT0, tT1;
float d2 = Distance.SegmentTriangleDistanceSq(out tS, out tT0, out tT1, seg, triangle);
if (d2 < capsuleTolR2)
{
Vector3 oldCapsuleStart = oldCapsule.Position;
Vector3 oldCapsuleEnd = oldCapsule.GetEnd();
Segment oldSeg = new Segment(oldCapsuleStart, oldCapsuleEnd - oldCapsuleStart);
d2 = Distance.SegmentTriangleDistanceSq(out tS, out tT0, out tT1, oldSeg, triangle);
// report result from old position
float dist = (float)System.Math.Sqrt(d2);
float depth = oldCapsule.Radius - dist;
Vector3 pt = triangle.GetPoint(tT0, tT1);
Vector3 collisionN = (d2 > JiggleMath.Epsilon) ? JiggleMath.NormalizeSafe(oldSeg.GetPoint(tS) - pt) :
meshTriangle.Plane.Normal;
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Reused existing collPts.
collPts[numCollPts].R0 = pt - body0Pos;
collPts[numCollPts].R1 = pt - body1Pos;
collPts[numCollPts++].InitialPenetration = depth;
}
collNormal += collisionN;
}
}
if (numCollPts > 0)
{
JiggleMath.NormalizeSafe(ref collNormal);
collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts);
}
#if USE_STACKALLOC
}
}
#else
}
FreeStackAlloc(potTriArray);
}
FreeStackAlloc(collPtArray);
#endif
}
}
/// <summary>
/// CollDetectCapsulseStaticMeshSweep
/// </summary>
/// <param name="oldCapsule"></param>
/// <param name="newCapsule"></param>
/// <param name="mesh"></param>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
private void CollDetectCapsulseStaticMeshSweep(Capsule oldCapsule, Capsule newCapsule,
TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
// really use a swept test - or overlap?
Vector3 delta = newCapsule.Position - oldCapsule.Position;
if (delta.LengthSquared < (0.25f * newCapsule.Radius * newCapsule.Radius))
{
CollDetectCapsuleStaticMeshOverlap(oldCapsule, newCapsule, mesh, info, collTolerance, collisionFunctor);
}
else
{
float capsuleLen = oldCapsule.Length;
float capsuleRadius = oldCapsule.Radius;
int nSpheres = 2 + (int)(capsuleLen / (2.0f * oldCapsule.Radius));
for (int iSphere = 0; iSphere < nSpheres; ++iSphere)
{
float offset = ((float)iSphere) * capsuleLen / ((float)nSpheres - 1.0f);
BoundingSphere oldSphere = new BoundingSphere(oldCapsule.Position + oldCapsule.Orientation.Backward() * offset, capsuleRadius);
BoundingSphere newSphere = new BoundingSphere(newCapsule.Position + newCapsule.Orientation.Backward() * offset, capsuleRadius);
CollDetectSphereStaticMesh.CollDetectSphereStaticMeshSweep(oldSphere, newSphere, mesh, info, collTolerance, collisionFunctor);
}
}
}
void GenerateCollisionMesh(VoxelGeometry geometry)
{
List<Vector3> vertices = new List<Vector3>(geometry.verts.Length);
List<TriangleVertexIndices> indices = new List<TriangleVertexIndices>(geometry.ib.Length / 3);
Matrix transform = this.Transformation.GetTransform();
for (int i = 0; i < geometry.verts.Length; i++)
vertices.Add(Vector3.Transform(geometry.verts[i].Position, transform));
for (int i = 0; i < geometry.ib.Length; i += 3)
{
TriangleVertexIndices tri = new TriangleVertexIndices(geometry.ib[i + 2], geometry.ib[i + 1], geometry.ib[i]);
indices.Add(tri);
}
TriangleMesh collisionMesh = new TriangleMesh(vertices, indices);
CollisionSkin collision = new CollisionSkin(null);
collision.AddPrimitive(collisionMesh, (int)MaterialTable.MaterialID.NotBouncyRough);
CollisionSkin collision2 = new CollisionSkin(null);
collision2.AddPrimitive(new JigLibX.Geometry.Plane(Vector3.Up, Transformation.GetBounds().Min.Y-3f), (int)MaterialTable.MaterialID.NotBouncyRough);
scene.GetPhysicsEngine().CollisionSystem.AddCollisionSkin(collision);
//scene.GetPhysicsEngine().CollisionSystem.AddCollisionSkin(collision2);
}
void CreateCollisionMesh(ModelPart collisionMesh)
{
SortedList<ushort, ushort> renamedVertexIndices = new SortedList<ushort, ushort>();
SortedList<ushort, ushort> renamedVertexIndicesCollision = new SortedList<ushort, ushort>();
RenderElement collisionElem = collisionMesh.renderElement;
List<Vector3> collisionVerts = new List<Vector3>();
List<TriangleVertexIndices> collisionIndices = new List<TriangleVertexIndices>(collisionElem.PrimitiveCount);
VertexPNTTI[] vertexData = new VertexPNTTI[vertexCount];
vertexBuffer.GetData<VertexPNTTI>(vertexData);
ushort[] indexDataCollision = new ushort[collisionElem.PrimitiveCount * 3];
collisionElem.IndexBuffer.GetData<ushort>(indexDataCollision);
for (int i = 0; i < collisionElem.PrimitiveCount; i++)
{
int index = i * 3;
for(int j = 0; j < 3; j++)
{
ushort currIdx = indexDataCollision[index + j];
if(!renamedVertexIndicesCollision.ContainsKey(currIdx))
{
renamedVertexIndicesCollision.Add(currIdx, (ushort)collisionVerts.Count);
Vector3 pos;
pos.X = vertexData[currIdx].Position.X;
pos.Y = vertexData[currIdx].Position.Y;
pos.Z = vertexData[currIdx].Position.Z;
collisionVerts.Add(pos);
}
}
ushort idx0 = indexDataCollision[index + 0];
ushort idx1 = indexDataCollision[index + 1];
ushort idx2 = indexDataCollision[index + 2];
collisionIndices.Add(new TriangleVertexIndices(renamedVertexIndicesCollision[idx2], renamedVertexIndicesCollision[idx1], renamedVertexIndicesCollision[idx0]));
}
/*
this.collisionMesh = new TriangleMesh();
this.collisionMesh.CreateMesh(collisionVerts, collisionIndices, 0, 0);
*/
this.collisionMesh = new TriangleMesh(collisionVerts, collisionIndices);
List<VertexPNTTI> newVertexData = new List<VertexPNTTI>();
for (int i = 0; i < parts.Length; i++)
{
RenderElement currElem = parts[i].renderElement;
ushort[] indexData = new ushort[currElem.PrimitiveCount * 3];
currElem.IndexBuffer.GetData<ushort>(indexData);
for (int j = 0; j < indexData.Length; j++)
{
if (!renamedVertexIndices.ContainsKey(indexData[j]))
{
renamedVertexIndices.Add(indexData[j], (ushort)newVertexData.Count);
newVertexData.Add(vertexData[indexData[j]]);
}
indexData[j] = renamedVertexIndices[indexData[j]];
}
currElem.IndexBuffer.SetData<ushort>(indexData);
}
vertexBuffer.Dispose();
vertexCount = newVertexData.Count;
vertexBuffer = new VertexBuffer(GFX.Device, VertexPNTTI.SizeInBytes * newVertexData.Count, BufferUsage.None);
vertexBuffer.SetData<VertexPNTTI>(newVertexData.ToArray());
for (int i = 0; i < parts.Length; i++)
parts[i].renderElement.VertexBuffer = vertexBuffer;
}
/// <summary>
/// DoOverlapBoxTriangleTest
/// </summary>
/// <param name="oldBox"></param>
/// <param name="newBox"></param>
/// <param name="triangle"></param>
/// <param name="mesh"></param>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
/// <returns>bool</returns>
private static bool DoOverlapBoxTriangleTest(Box oldBox, Box newBox,
ref IndexedTriangle triangle, TriangleMesh mesh,
ref CollDetectInfo info, float collTolerance,
CollisionFunctor collisionFunctor)
{
Matrix4 dirs0 = newBox.Orientation;
#region REFERENCE: Triangle tri = new Triangle(mesh.GetVertex(triangle.GetVertexIndex(0)),mesh.GetVertex(triangle.GetVertexIndex(1)),mesh.GetVertex(triangle.GetVertexIndex(2)));
Vector3 triVec0;
Vector3 triVec1;
Vector3 triVec2;
mesh.GetVertex(triangle.GetVertexIndex(0), out triVec0);
mesh.GetVertex(triangle.GetVertexIndex(1), out triVec1);
mesh.GetVertex(triangle.GetVertexIndex(2), out triVec2);
// Deano move tri into world space
Matrix4 transformMatrix = mesh.TransformMatrix;
Vector3.Transform(ref triVec0, ref transformMatrix, out triVec0);
Vector3.Transform(ref triVec1, ref transformMatrix, out triVec1);
Vector3.Transform(ref triVec2, ref transformMatrix, out triVec2);
Triangle tri = new Triangle(ref triVec0,ref triVec1,ref triVec2);
#endregion
#region REFERENCE Vector3 triEdge0 = (tri.GetPoint(1) - tri.GetPoint(0));
Vector3 pt0;
Vector3 pt1;
tri.GetPoint(0, out pt0);
tri.GetPoint(1, out pt1);
Vector3 triEdge0;
Vector3.Subtract(ref pt1, ref pt0, out triEdge0);
#endregion
#region REFERENCE Vector3 triEdge1 = (tri.GetPoint(2) - tri.GetPoint(1));
Vector3 pt2;
tri.GetPoint(2, out pt2);
Vector3 triEdge1;
Vector3.Subtract(ref pt2, ref pt1, out triEdge1);
#endregion
#region REFERENCE Vector3 triEdge2 = (tri.GetPoint(0) - tri.GetPoint(2));
Vector3 triEdge2;
Vector3.Subtract(ref pt0, ref pt2, out triEdge2);
#endregion
triEdge0.Normalize();
triEdge1.Normalize();
triEdge2.Normalize();
// BEN-OPTIMISATION: Replaced loops with code that requires no looping.
// The new code is faster, has less allocations and math especially
// since the method returns as soon as it finds a non-overlapping axis,
// i.e. Before irreleveat allocations occur.
#region "Old (less efficient) code"
/*Vector3 triNormal = triangle.Plane.Normal;
// the 15 potential separating axes
const int numAxes = 13;
Vector3[] axes = new Vector3[numAxes];
axes[0] = triNormal;
axes[1] = dirs0.Right;
axes[2] = dirs0.Up;
axes[3] = dirs0.Backward;
Vector3.Cross(ref axes[1], ref triEdge0, out axes[4]);
Vector3.Cross(ref axes[1], ref triEdge1, out axes[5]);
Vector3.Cross(ref axes[1], ref triEdge2, out axes[6]);
Vector3.Cross(ref axes[2], ref triEdge0, out axes[7]);
Vector3.Cross(ref axes[2], ref triEdge1, out axes[8]);
Vector3.Cross(ref axes[2], ref triEdge2, out axes[9]);
Vector3.Cross(ref axes[3], ref triEdge0, out axes[10]);
Vector3.Cross(ref axes[3], ref triEdge1, out axes[11]);
Vector3.Cross(ref axes[3], ref triEdge2, out axes[12]);
// the overlap depths along each axis
float[] overlapDepths = new float[numAxes];
// see if the boxes are separate along any axis, and if not keep a
// record of the depths along each axis
int i;
for (i = 0; i < numAxes; ++i)
{
overlapDepths[i] = 1.0f;
if (Disjoint(out overlapDepths[i], axes[i], newBox, tri, collTolerance))
return false;
}
// The box overlap, find the separation depth closest to 0.
float minDepth = float.MaxValue;
int minAxis = -1;
for (i = 0; i < numAxes; ++i)
{
// If we can't normalise the axis, skip it
float l2 = axes[i].LengthSquared;
if (l2 < JiggleMath.Epsilon)
continue;
// Normalise the separation axis and the depth
float invl = 1.0f / (float)System.Math.Sqrt(l2);
axes[i] *= invl;
overlapDepths[i] *= invl;
// If this axis is the minimum, select it
if (overlapDepths[i] < minDepth)
{
minDepth = overlapDepths[i];
minAxis = i;
}
}
if (minAxis == -1)
return false;
// Make sure the axis is facing towards the 0th box.
// if not, invert it
Vector3 D = newBox.GetCentre() - tri.Centre;
Vector3 N = axes[minAxis];
float depth = overlapDepths[minAxis];*/
#endregion
#region "Optimised code"
Vector3 triNormal = triangle.Plane.Normal;
Vector3 right = dirs0.Right();
Vector3 up = dirs0.Up();
Vector3 backward = dirs0.Backward();
float testDepth;
if (Disjoint(out testDepth, ref triNormal, newBox, ref tri, collTolerance))
return (false);
float depth = testDepth;
Vector3 N = triNormal;
if (Disjoint(out testDepth, ref right, newBox, ref tri, collTolerance))
return (false);
if (testDepth < depth)
{
depth = testDepth;
N = right;
}
if (Disjoint(out testDepth, ref up, newBox, ref tri, collTolerance))
return (false);
if (testDepth < depth)
{
depth = testDepth;
N = up;
}
if (Disjoint(out testDepth, ref backward, newBox, ref tri, collTolerance))
return (false);
if (testDepth < depth)
{
depth = testDepth;
N = backward;
}
Vector3 axis;
Vector3.Cross(ref right, ref triEdge0, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
Vector3.Cross(ref right, ref triEdge1, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
Vector3.Cross(ref right, ref triEdge2, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
Vector3.Cross(ref up, ref triEdge0, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
Vector3.Cross(ref up, ref triEdge1, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
Vector3.Cross(ref up, ref triEdge2, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
Vector3.Cross(ref backward, ref triEdge0, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
Vector3.Cross(ref backward, ref triEdge1, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
Vector3.Cross(ref backward, ref triEdge2, out axis);
if (Disjoint(out testDepth, ref axis, newBox, ref tri, collTolerance))
return (false);
testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
if (testDepth < depth)
{
depth = testDepth;
N = axis;
}
/*if (N == Vector3.Zero)
return (false);*/
Vector3 D = newBox.GetCentre() - tri.Centre;
N.Normalize();
int i;
#endregion
if (Vector3.Dot(D, N) < 0.0f)
N *= -1;
Vector3 boxOldPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 boxNewPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.Position : Vector3.Zero;
Vector3 meshPos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
List<Vector3> pts = new List<Vector3>();
//pts.Clear();
const float combinationDist = 0.05f;
GetBoxTriangleIntersectionPoints(pts, newBox, tri, depth + combinationDist);
// adjust the depth
#region REFERENCE: Vector3 delta = boxNewPos - boxOldPos;
Vector3 delta;
Vector3.Subtract(ref boxNewPos, ref boxOldPos, out delta);
#endregion
#region REFERENCE: float oldDepth = depth + Vector3.Dot(delta, N);
float oldDepth;
Vector3.Dot(ref delta, ref N, out oldDepth);
oldDepth += depth;
#endregion
unsafe
{
// report collisions
int numPts = pts.Count;
#if USE_STACKALLOC
SmallCollPointInfo* collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed (SmallCollPointInfo* collPts = collPtArray)
#endif
{
if (numPts > 0)
{
if (numPts >= MaxLocalStackSCPI)
{
numPts = MaxLocalStackSCPI - 1;
}
// adjust positions
for (i = 0; i < numPts; ++i)
{
// BEN-OPTIMISATION: Reused existing SmallCollPointInfo and inlined vector substraction.
collPts[i].R0.X = pts[i].X - boxNewPos.X;
collPts[i].R0.Y = pts[i].Y - boxNewPos.Y;
collPts[i].R0.Z = pts[i].Z - boxNewPos.Z;
collPts[i].R1.X = pts[i].X - meshPos.X;
collPts[i].R1.Y = pts[i].Y - meshPos.Y;
collPts[i].R1.Z = pts[i].Z - meshPos.Z;
collPts[i].InitialPenetration = oldDepth;
}
collisionFunctor.CollisionNotify(ref info, ref N, collPts, numPts);
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
return true;
}
else
{
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
return false;
}
}
}
}
public Terrain( Vector3 posCenter,Vector3 size, int cellsX, int cellsZ, GraphicsDevice g, Texture2D tex)
: base()
{
numVertsX = cellsX + 1;
numVertsZ = cellsZ + 1;
numVerts = numVertsX * numVertsZ;
numTriX = cellsX * 2;
numTriZ = cellsZ;
numTris = numTriX * numTriZ;
verts = new VertexPositionNormalTexture[numVerts];
int numIndices = numTris * 3;
indices = new int[numIndices];
float cellSizeX = (float)size.X / cellsX;
float cellSizeZ = (float)size.Z / cellsZ;
texture = tex;
Random r = new Random();
Random r2 = new Random(DateTime.Now.Millisecond);
double targetHeight = 0;
double currentHeight = 0;
// Fill in the vertices
int count = 0;
//float edgeHeigh = 0;
//float worldZPosition = posCenter.Z - (size.Z / 2);
float worldZPosition = - (size.Z / 2);
float height;
float stair = 0;
float diff = .5f;
for (int z = 0; z < numVertsZ; z++)
{
//float worldXPosition = posCenter.X - (size.X / 2);
float worldXPosition = - (size.X / 2);
for (int x = 0; x < numVertsX; x++)
{
if (count % numVertsX == 0)
targetHeight = r2.NextDouble();
//targetHeight += Math.Abs(worldZPosition);// +worldXPosition * 1.0f;
currentHeight += (targetHeight - currentHeight) * .009f;
//if(x!=0)
//currentHeight = (targetHeight) / ((float)x / (float)(numVertsX+1));
//height = (float)((r.NextDouble() + currentHeight) * size.Y);
//height = 1;
//stair += diff;
//if (z + x == 17)
//stair += 10;
//height += stair;
verts[count].Position = new Vector3(worldXPosition,(float)currentHeight, worldZPosition);
verts[count].Normal = Vector3.Zero;
verts[count].TextureCoordinate.X = (float)x / (numVertsX - 1);
verts[count].TextureCoordinate.Y = (float)z / (numVertsZ - 1);
//if (z + x == 17)
//stair -= 10;
count++;
// Advance in x
worldXPosition += cellSizeX;
}
currentHeight = 0;
// Advance in z
worldZPosition += cellSizeZ;
//diff *= -1;
//if (diff < 1)
// stair += numVertsX * diff;
}
int index = 0;
int startVertex = 0;
for (int cellZ = 0; cellZ < cellsZ; cellZ++)
{
for (int cellX = 0; cellX < cellsX; cellX++)
{
indices[index] = startVertex + 0;
indices[index + 1] = startVertex + 1;
indices[index + 2] = startVertex + numVertsX;
SetNormalOfTriangleAtIndices(indices[index], indices[index + 1], indices[index + 2]);
meshIndices.Add(new TriangleVertexIndices(index, index + 1, index + 2));
meshVertices.Add(verts[indices[index]].Position);
meshVertices.Add(verts[indices[index+1]].Position);
meshVertices.Add(verts[indices[index+2]].Position);
index += 3;
indices[index] = startVertex + 1;
indices[index + 1] = startVertex + numVertsX + 1;
indices[index + 2] = startVertex + numVertsX;
SetNormalOfTriangleAtIndices(indices[index], indices[index + 1], indices[index + 2]);
meshIndices.Add(new TriangleVertexIndices(index, index + 1, index + 2));
meshVertices.Add(verts[indices[index]].Position);
meshVertices.Add(verts[indices[index + 1]].Position);
meshVertices.Add(verts[indices[index + 2]].Position);
index += 3;
startVertex++;
}
startVertex++;
}
try
{
Effect = new BasicEffect(g);
mesh = new TriangleMesh();
//mesh.CreateMesh(meshVertices, meshIndices, 2, cellSizeX);
}
catch (Exception E)
{
}
this.Body = new Body();
Skin = new CollisionSkin(Body);
Body.CollisionSkin = Skin;
Body.ExternalData = this;
float heightf = 0;
try
{
Array2D field = new Array2D(numVertsX, numVertsZ);
int i = 0;
for (int c = 0; c < verts.Length; c++)
{
int x = c / numVertsX;
int z = c % numVertsX;
if (i >= verts.Length)
i = (i % verts.Length)+1;
heightf = verts[i].Position.Y + posCenter.Y;
//heightf = verts[i].Position.Y;
i += numVertsX;
field.SetAt(x,z, heightf);
}
// Body.MoveTo(Position, Matrix.Identity);
Heightmap hm = new Heightmap(field,
(-size.X / 2) / (cellsX+0) + cellSizeX/2,
(-size.Z / 2) / (cellsZ + 0) + cellSizeZ/2,
size.X / (cellsX+0),
size.Z / (cellsZ+0));
Skin.AddPrimitive(hm, new MaterialProperties(0.7f, 0.7f, 0.6f));
//Skin.AddPrimitive(GetMesh(), new MaterialProperties(0.7f, 0.7f, 0.6f));
//VertexPositionColor[] wireFrame = Skin.GetLocalSkinWireframe(); // 1200 across before Z changes to from -7.5/-7.35 to -7.35/-7.2
PhysicsSystem.CurrentPhysicsSystem.CollisionSystem.AddCollisionSkin(Skin);
CommonInit(posCenter, new Vector3(0,0,0), null, false);
}
catch(Exception E)
{
}
}
private TriangleMesh GetTriangleMesh(Model model, Transform3D transform)
{
TriangleMesh triangleMesh = new TriangleMesh();
List<Vector3> vertexList = new List<Vector3>();
List<TriangleVertexIndices> indexList = new List<TriangleVertexIndices>();
ExtractData(vertexList, indexList, model);
for (int i = 0; i < vertexList.Count; i++)
{
vertexList[i] = Vector3.Transform(vertexList[i], transform.World);
}
// create the collision mesh
triangleMesh.CreateMesh(vertexList, indexList, 1, 1.0f);
return triangleMesh;
}
void GenerateCollisionMesh()
{
List<Vector3> vertColl = new List<Vector3>();
for (int i = 0; i < geometry.verts.Length; i++)
{
vertColl.Add(Vector3.Transform(new Vector3(geometry.verts[i].Position.X, geometry.verts[i].Position.Y, geometry.verts[i].Position.Z), transformation.GetTransform()));
//vertColl[i] = Vector3.Transform(new Vector3(geometry.verts[i].Position.X, geometry.verts[i].Position.Y, geometry.verts[i].Position.Z), transformation.GetTransform());
}
int triCount = 0;
TriangleVertexIndices triIdx = new TriangleVertexIndices(0, 0, 0);
List<TriangleVertexIndices> triColl = new List<TriangleVertexIndices>();
for (int i = 0; i < geometry.ib.Length; i++)
{
//int index = geometry.ib[i];
//vertColl.Add(Vector3.Transform(new Vector3(geometry.verts[index].Position.X, geometry.verts[index].Position.Y, geometry.verts[index].Position.Z), transformation.GetTransform()));
switch (triCount)
{
case 0:
triIdx.I2 = geometry.ib[i];
break;
case 1:
triIdx.I1 = geometry.ib[i];
break;
case 2:
triIdx.I0 = geometry.ib[i];
triCount = -1;
triColl.Add(triIdx);
triIdx = new TriangleVertexIndices(0, 0, 0);
break;
}
triCount++;
}
CollisionMesh = new TriangleMesh();
CollisionMesh.CreateMesh(vertColl.ToArray(), triColl.ToArray(), 1500, 0.01f);
Collision = new CollisionSkin(null);
Collision.AddPrimitive(CollisionMesh, (int)MaterialTable.MaterialID.NotBouncyRough);
PhysicsSystem.CurrentPhysicsSystem.CollisionSystem.AddCollisionSkin(Collision);
}
internal static void CollDetectSphereStaticMeshSweep(BoundingSphere oldSphere, BoundingSphere newSphere, TriangleMesh mesh,
CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
// really use a swept test - or overlap?
Vector3 delta = newSphere.Center - oldSphere.Center;
if (delta.LengthSquared() < (0.25f * newSphere.Radius * newSphere.Radius))
{
CollDetectSphereStaticMeshOverlap(oldSphere, newSphere, mesh, info, collTolerance, collisionFunctor);
}
else
{
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
float sphereTolR = collTolerance + oldSphere.Radius;
float sphereToR2 = sphereTolR * sphereTolR;
Vector3 collNormal = Vector3.Zero;
BoundingBox bb = BoundingBoxHelper.InitialBox;
BoundingBoxHelper.AddSphere(oldSphere, ref bb);
BoundingBoxHelper.AddSphere(newSphere, ref bb);
// get the spheres centers in triangle mesh space
Vector3 newSphereCen = Vector3.Transform(newSphere.Center, mesh.InverseTransformMatrix);
Vector3 oldSphereCen = Vector3.Transform(oldSphere.Center, mesh.InverseTransformMatrix);
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo* collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
int* potentialTriangles = stackalloc int[MaxLocalStackTris];
{
{
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed (SmallCollPointInfo* collPts = collPtArray)
{
int[] potTriArray = IntStackAlloc();
fixed( int* potentialTriangles = potTriArray)
{
#endif
int numCollPts = 0;
int numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb);
for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle)
{
// first test the old sphere for being on the wrong side
IndexedTriangle meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]);
float distToCentreOld = meshTriangle.Plane.DotCoordinate(oldSphereCen);
if (distToCentreOld <= 0.0f)
continue;
// now test the new sphere for being clear
float distToCentreNew = meshTriangle.Plane.DotCoordinate(newSphereCen);
if (distToCentreNew > sphereTolR)
continue;
int i0, i1, i2;
meshTriangle.GetVertexIndices(out i0, out i1, out i2);
Triangle triangle = new Triangle(mesh.GetVertex(i0), mesh.GetVertex(i1), mesh.GetVertex(i2));
// If the old sphere is intersecting, just use that result
float s, t;
float d2 = Distance.PointTriangleDistanceSq(out s, out t, oldSphereCen, triangle);
if (d2 < sphereToR2)
{
float dist = (float)System.Math.Sqrt(d2);
float depth = oldSphere.Radius - dist;
Vector3 triangleN = triangle.Normal;
Vector3 normSafe = oldSphereCen - triangle.GetPoint(s, t);
JiggleMath.NormalizeSafe(ref normSafe);
Vector3 collisionN = (dist > float.Epsilon) ? normSafe : triangleN;
// since impulse gets applied at the old position
Vector3 pt = oldSphere.Center - oldSphere.Radius * collisionN;
if (numCollPts < MaxLocalStackSCPI)
{
collPts[numCollPts++] = new SmallCollPointInfo(pt - body0Pos, pt - body1Pos, depth);
}
collNormal += collisionN;
}
else if (distToCentreNew < distToCentreOld)
{
// old sphere is not intersecting - do a sweep, but only if the sphere is moving into the
// triangle
Vector3 pt, N; // CHECK THIS
float depth;
if (Intersection.SweptSphereTriangleIntersection(out pt, out N, out depth, oldSphere, newSphere, triangle,
distToCentreOld, distToCentreNew, Intersection.EdgesToTest.EdgeAll, Intersection.CornersToTest.CornerAll))
{
// collision point etc must be relative to the old position because that's
//where the impulses are applied
float dist = (float)System.Math.Sqrt(d2);
float depth2 = oldSphere.Radius - dist;
Vector3 triangleN = triangle.Normal;
Vector3 normSafe = oldSphereCen - triangle.GetPoint(s, t);
JiggleMath.NormalizeSafe(ref normSafe);
Vector3 collisionN = (dist > JiggleMath.Epsilon) ? normSafe : triangleN;
// since impulse gets applied at the old position
Vector3 pt2 = oldSphere.Center - oldSphere.Radius * collisionN;
if (numCollPts < MaxLocalStackSCPI)
{
collPts[numCollPts++] = new SmallCollPointInfo(pt2 - body0Pos, pt2 - body1Pos, depth);
}
collNormal += collisionN;
}
}
}
if (numCollPts > 0)
{
JiggleMath.NormalizeSafe(ref collNormal);
collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts);
}
}
#if USE_STACKALLOC
}
}
#else
FreeStackAlloc(potTriArray);
}
FreeStackAlloc(collPtArray);
}
#endif
}
}
public Terrain( Vector3 posCenter,Vector3 size, int cellsX, int cellsZ, GraphicsDevice g, Texture2D tex)
: base()
{
numVertsX = cellsX + 1;
numVertsZ = cellsZ + 1;
numVerts = numVertsX * numVertsZ;
numTriX = cellsX * 2;
numTriZ = cellsZ;
numTris = numTriX * numTriZ;
verts = new VertexPositionNormalTexture[numVerts];
int numIndices = numTris * 3;
indices = new int[numIndices];
float cellSizeX = (float)size.X / cellsX;
float cellSizeZ = (float)size.Z / cellsZ;
texture = tex;
Random r = new Random();
Random r2 = new Random(DateTime.Now.Millisecond);
double targetHeight = 0;
double currentHeight = 0;
// Fill in the vertices
int count = 0;
// distribute height nodes across the map.
// for each vert, calculate the height by summing (node height / distance)
int numHeightNodes = 10;
heightNodes = new Vector3[numHeightNodes];
for (int i = 0; i < numHeightNodes; i++)
{
heightNodes[i] = new Vector3((float)((-size.X / 2) + (r.NextDouble() * size.X)),
(float)(r.NextDouble() * 0),
(float)((-size.Z / 2) + (r.NextDouble() * size.Z)));
}
bool stretchTexture=false;
float textureTileCount=10000;
float worldZPosition = - (size.Z / 2);
for (int z = 0; z < numVertsZ; z++)
{
float worldXPosition = - (size.X / 2);
for (int x = 0; x < numVertsX; x++)
{
if (count % numVertsX == 0)
targetHeight = r2.NextDouble();
//currentHeight += (targetHeight - currentHeight) * .009f;
float height = GetNodeBasedHeight(worldXPosition, worldZPosition);
height = 0;
verts[count].Position = new Vector3(worldXPosition, height, worldZPosition);
verts[count].Normal = Vector3.Zero;
if (stretchTexture)
{
verts[count].TextureCoordinate.X = (float)x / (numVertsX - 1);
verts[count].TextureCoordinate.Y = (float)z / (numVertsZ - 1);
}
else
{
verts[count].TextureCoordinate.X = ((float)x / (numVertsX - 1))*textureTileCount;
verts[count].TextureCoordinate.Y = ((float)z / (numVertsZ - 1))*textureTileCount;
}
count++;
// Advance in x
worldXPosition += cellSizeX;
}
currentHeight = 0;
// Advance in z
worldZPosition += cellSizeZ;
}
int index = 0;
int startVertex = 0;
for (int cellZ = 0; cellZ < cellsZ; cellZ++)
{
for (int cellX = 0; cellX < cellsX; cellX++)
{
indices[index] = startVertex + 0;
indices[index + 1] = startVertex + 1;
indices[index + 2] = startVertex + numVertsX;
SetNormalOfTriangleAtIndices(indices[index], indices[index + 1], indices[index + 2]);
meshIndices.Add(new TriangleVertexIndices(index, index + 1, index + 2));
meshVertices.Add(verts[indices[index]].Position);
meshVertices.Add(verts[indices[index+1]].Position);
meshVertices.Add(verts[indices[index+2]].Position);
index += 3;
indices[index] = startVertex + 1;
indices[index + 1] = startVertex + numVertsX + 1;
indices[index + 2] = startVertex + numVertsX;
SetNormalOfTriangleAtIndices(indices[index], indices[index + 1], indices[index + 2]);
meshIndices.Add(new TriangleVertexIndices(index, index + 1, index + 2));
meshVertices.Add(verts[indices[index]].Position);
meshVertices.Add(verts[indices[index + 1]].Position);
meshVertices.Add(verts[indices[index + 2]].Position);
index += 3;
startVertex++;
}
startVertex++;
}
try
{
Effect = new BasicEffect(g);
mesh = new TriangleMesh();
//mesh.CreateMesh(meshVertices, meshIndices, 2, cellSizeX);
}
catch (Exception E)
{
System.Diagnostics.Debug.WriteLine(E.StackTrace);
}
this.Body = new Body();
Skin = new CollisionSkin(Body);
Body.CollisionSkin = Skin;
Body.ExternalData = this;
float heightf = 0;
try
{
Array2D field = new Array2D(numVertsX, numVertsZ);
int i = 0;
for (int c = 0; c < verts.Length; c++)
{
int x = c / numVertsX;
int z = c % numVertsX;
if (i >= verts.Length)
i = (i % verts.Length)+1;
heightf = verts[i].Position.Y + posCenter.Y; // works
//heightf = verts[i].Position.Y;
i += numVertsX;
field.SetAt(x,z, heightf);
}
// Body.MoveTo(Position, Matrix.Identity);
Heightmap hm = new Heightmap(field,
(-size.X / 2) / (cellsX+0) + cellSizeX/2,
(-size.Z / 2) / (cellsZ + 0) + cellSizeZ/2,
size.X / (cellsX+0),
size.Z / (cellsZ+0));
Skin.AddPrimitive(hm, new MaterialProperties(0.7f, 0.7f, 0.6f));
//Skin.AddPrimitive(GetMesh(), new MaterialProperties(0.7f, 0.7f, 0.6f));
//VertexPositionColor[] wireFrame = Skin.GetLocalSkinWireframe(); // 1200 across before Z changes to from -7.5/-7.35 to -7.35/-7.2
PhysicsSystem.CurrentPhysicsSystem.CollisionSystem.AddCollisionSkin(Skin);
CommonInit(posCenter, new Vector3(0,0,0), null, false, 0);
}
catch(Exception E)
{
System.Diagnostics.Debug.WriteLine(E.StackTrace);
}
}
public static void CollDetectSphereStaticMeshOverlap(BoundingSphere oldSphere, BoundingSphere newSphere,
TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
float sphereTolR = collTolerance + newSphere.Radius;
float sphereTolR2 = sphereTolR * sphereTolR;
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo* collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
int* potentialTriangles = stackalloc int[MaxLocalStackTris];
{
{
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed (SmallCollPointInfo* collPts = collPtArray)
{
int[] potTriArray = IntStackAlloc();
fixed( int* potentialTriangles = potTriArray)
{
#endif
int numCollPts = 0;
Vector3 collNormal = Vector3.Zero;
BoundingBox bb = BoundingBoxHelper.InitialBox;
BoundingBoxHelper.AddSphere(newSphere, ref bb);
int numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb);
// Deano : get the spheres centers in triangle mesh space
Vector3 newSphereCen = Vector3.Transform(newSphere.Center, mesh.InverseTransformMatrix);
Vector3 oldSphereCen = Vector3.Transform(oldSphere.Center, mesh.InverseTransformMatrix);
for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle)
{
IndexedTriangle meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]);
float distToCentre = meshTriangle.Plane.DotCoordinate(newSphereCen);
if (distToCentre <= 0.0f)
continue;
if (distToCentre >= sphereTolR)
continue;
int i0, i1, i2;
meshTriangle.GetVertexIndices(out i0, out i1, out i2);
Triangle triangle = new Triangle(mesh.GetVertex(i0), mesh.GetVertex(i1), mesh.GetVertex(i2));
float s, t;
float newD2 = Distance.PointTriangleDistanceSq(out s, out t, newSphereCen, triangle);
if (newD2 < sphereTolR2)
{
// have overlap - but actually report the old intersection
float oldD2 = Distance.PointTriangleDistanceSq(out s, out t, oldSphereCen, triangle);
float dist = (float)System.Math.Sqrt((float)oldD2);
float depth = oldSphere.Radius - dist;
Vector3 triPointSTNorm = oldSphereCen - triangle.GetPoint(s, t);
JiggleMath.NormalizeSafe(ref triPointSTNorm);
Vector3 collisionN = (dist > float.Epsilon) ? triPointSTNorm : triangle.Normal;
// since impulse get applied at the old position
Vector3 pt = oldSphere.Center - oldSphere.Radius * collisionN;
if (numCollPts < MaxLocalStackSCPI)
{
collPts[numCollPts++] = new SmallCollPointInfo(pt - body0Pos, pt - body1Pos, depth);
}
collNormal += collisionN;
}
}
if (numCollPts > 0)
{
JiggleMath.NormalizeSafe(ref collNormal);
collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts);
}
#if USE_STACKALLOC
}
}
#else
FreeStackAlloc(potTriArray);
}
FreeStackAlloc(collPtArray);
}
#endif
}
}
private static bool DoOverlapBoxTriangleTest(Box oldBox, Box newBox,
ref IndexedTriangle triangle, TriangleMesh mesh,
ref CollDetectInfo info, float collTolerance,
CollisionFunctor collisionFunctor)
{
Matrix dirs0 = newBox.Orientation;
#region REFERENCE: Triangle tri = new Triangle(mesh.GetVertex(triangle.GetVertexIndex(0)),mesh.GetVertex(triangle.GetVertexIndex(1)),mesh.GetVertex(triangle.GetVertexIndex(2)));
Vector3 triVec0;
Vector3 triVec1;
Vector3 triVec2;
mesh.GetVertex(triangle.GetVertexIndex(0), out triVec0);
mesh.GetVertex(triangle.GetVertexIndex(1), out triVec1);
mesh.GetVertex(triangle.GetVertexIndex(2), out triVec2);
// Deano move tri into world space
Matrix transformMatrix = mesh.TransformMatrix;
Vector3.Transform(ref triVec0, ref transformMatrix, out triVec0);
Vector3.Transform(ref triVec1, ref transformMatrix, out triVec1);
Vector3.Transform(ref triVec2, ref transformMatrix, out triVec2);
Triangle tri = new Triangle(ref triVec0,ref triVec1,ref triVec2);
#endregion
#region REFERENCE Vector3 triEdge0 = (tri.GetPoint(1) - tri.GetPoint(0));
Vector3 pt0;
Vector3 pt1;
tri.GetPoint(0, out pt0);
tri.GetPoint(1, out pt1);
Vector3 triEdge0;
Vector3.Subtract(ref pt1, ref pt0, out triEdge0);
#endregion
#region REFERENCE Vector3 triEdge1 = (tri.GetPoint(2) - tri.GetPoint(1));
Vector3 pt2;
tri.GetPoint(2, out pt2);
Vector3 triEdge1;
Vector3.Subtract(ref pt2, ref pt1, out triEdge1);
#endregion
#region REFERENCE Vector3 triEdge2 = (tri.GetPoint(0) - tri.GetPoint(2));
Vector3 triEdge2;
Vector3.Subtract(ref pt0, ref pt2, out triEdge2);
#endregion
triEdge0.Normalize();
triEdge1.Normalize();
triEdge2.Normalize();
Vector3 triNormal = triangle.Plane.Normal;
// the 15 potential separating axes
const int numAxes = 13;
Vector3[] axes = new Vector3[numAxes];
axes[0] = triNormal;
axes[1] = dirs0.Right;
axes[2] = dirs0.Up;
axes[3] = dirs0.Backward;
Vector3.Cross(ref axes[1], ref triEdge0, out axes[4]);
Vector3.Cross(ref axes[1], ref triEdge1, out axes[5]);
Vector3.Cross(ref axes[1], ref triEdge2, out axes[6]);
Vector3.Cross(ref axes[2], ref triEdge0, out axes[7]);
Vector3.Cross(ref axes[2], ref triEdge1, out axes[8]);
Vector3.Cross(ref axes[2], ref triEdge2, out axes[9]);
Vector3.Cross(ref axes[3], ref triEdge0, out axes[10]);
Vector3.Cross(ref axes[3], ref triEdge1, out axes[11]);
Vector3.Cross(ref axes[3], ref triEdge2, out axes[12]);
// the overlap depths along each axis
float[] overlapDepths = new float[numAxes];
// see if the boxes are separate along any axis, and if not keep a
// record of the depths along each axis
int i;
for (i = 0; i < numAxes; ++i)
{
overlapDepths[i] = 1.0f;
if (Disjoint(out overlapDepths[i], axes[i], newBox, tri, collTolerance))
return false;
}
// The box overlap, find the separation depth closest to 0.
float minDepth = float.MaxValue;
int minAxis = -1;
for (i = 0; i < numAxes; ++i)
{
// If we can't normalise the axis, skip it
float l2 = axes[i].LengthSquared();
if (l2 < JiggleMath.Epsilon)
continue;
// Normalise the separation axis and the depth
float invl = 1.0f / (float)System.Math.Sqrt(l2);
axes[i] *= invl;
overlapDepths[i] *= invl;
// If this axis is the minimum, select it
if (overlapDepths[i] < minDepth)
{
minDepth = overlapDepths[i];
minAxis = i;
}
}
if (minAxis == -1)
return false;
// Make sure the axis is facing towards the 0th box.
// if not, invert it
Vector3 D = newBox.GetCentre() - tri.Centre;
Vector3 N = axes[minAxis];
float depth = overlapDepths[minAxis];
if (Vector3.Dot(D, N) < 0.0f)
N *= -1;
Vector3 boxOldPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 boxNewPos = (info.Skin0.Owner != null) ? info.Skin0.Owner.Position : Vector3.Zero;
Vector3 meshPos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
List<Vector3> pts = new List<Vector3>();
//pts.Clear();
const float combinationDist = 0.05f;
GetBoxTriangleIntersectionPoints(pts, newBox, tri, depth + combinationDist);
// adjust the depth
#region REFERENCE: Vector3 delta = boxNewPos - boxOldPos;
Vector3 delta;
Vector3.Subtract(ref boxNewPos, ref boxOldPos, out delta);
#endregion
#region REFERENCE: float oldDepth = depth + Vector3.Dot(delta, N);
float oldDepth;
Vector3.Dot(ref delta, ref N, out oldDepth);
oldDepth += depth;
#endregion
unsafe
{
// report collisions
int numPts = pts.Count;
#if USE_STACKALLOC
SmallCollPointInfo* collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed (SmallCollPointInfo* collPts = collPtArray)
#endif
{
if (numPts > 0)
{
if (numPts >= MaxLocalStackSCPI)
{
numPts = MaxLocalStackSCPI - 1;
}
// adjust positions
for (i = 0; i < numPts; ++i)
{
collPts[i] = new SmallCollPointInfo(pts[i] - boxNewPos, pts[i] - meshPos, oldDepth);
}
collisionFunctor.CollisionNotify(ref info, ref N, collPts, numPts);
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
return true;
}
else
{
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
return false;
}
}
}
}
/// <summary>
/// CollDetectBoxStaticMeshOverlap
/// </summary>
/// <param name="oldBox"></param>
/// <param name="newBox"></param>
/// <param name="mesh"></param>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
/// <returns>bool</returns>
private static bool CollDetectBoxStaticMeshOverlap(Box oldBox,
Box newBox,
TriangleMesh mesh,
ref CollDetectInfo info,
float collTolerance,
CollisionFunctor collisionFunctor)
{
float boxRadius = newBox.GetBoundingRadiusAroundCentre();
#region REFERENCE: Vector3 boxCentre = newBox.GetCentre();
Vector3 boxCentre;
newBox.GetCentre(out boxCentre);
// Deano need to trasnform the box center into mesh space
Matrix4 invTransformMatrix = mesh.InverseTransformMatrix;
Vector3.Transform(ref boxCentre, ref invTransformMatrix, out boxCentre);
#endregion
BoundingBox bb = BoundingBoxHelper.InitialBox;
BoundingBoxHelper.AddBox(newBox, ref bb);
unsafe
{
bool collision = false;
#if USE_STACKALLOC
int* potentialTriangles = stackalloc int[MaxLocalStackTris];
{
#else
int[] potTriArray = IntStackAlloc();
fixed( int* potentialTriangles = potTriArray)
{
#endif
// aabox is done in mesh space and handles the mesh transform correctly
int numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb);
for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle)
{
IndexedTriangle meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]);
// quick early test is done in mesh space
float dist = meshTriangle.Plane.DotCoordinate(boxCentre);
// BEN-BUG-FIX: Fixed by chaning 0.0F to -boxRadius.
if (dist > boxRadius || dist < -boxRadius)
continue;
if (DoOverlapBoxTriangleTest(
oldBox, newBox,
ref meshTriangle,
mesh,
ref info,
collTolerance,
collisionFunctor))
{
collision = true;
}
}
#if USE_STACKALLOC
}
#else
}
FreeStackAlloc(potTriArray);
#endif
return collision;
}
}
void ModifyMesh()
{
List<ModelPart> meshes = new List<ModelPart>();
ModelPart collisionMesh = null;
List<ModelPart> interactables = new List<ModelPart>();
for (int i = 0; i < parts.Length; i++)
{
string[] meshName = parts[i].name.Split(':');
if (meshName.Length == 1)
{
if (meshName[0] == "COLLISION")
collisionMesh = parts[i];
else
meshes.Add(parts[i]);
}
else
{
if (meshName[0] == "INTERACT")
{
interactables.Add(parts[i]);
}
else
{
if (!LODS.ContainsKey(meshName[0]))
LODS.Add(meshName[0], new List<ModelPart>());
int currLODValue = int.Parse(meshName[1].Substring(3));
parts[i].name = currLODValue.ToString();
bool addedPart = false;
for (int j = 0; j < LODS[meshName[0]].Count; j++)
{
int LODValue = int.Parse(LODS[meshName[0]][j].name);
if (currLODValue < LODValue)
{
LODS[meshName[0]].Insert(j, parts[i]);
addedPart = true;
break;
}
}
if (!addedPart)
LODS[meshName[0]].Add(parts[i]);
}
}
}
if (collisionMesh != null)
CreateCollisionMesh(collisionMesh);
interactNodes = new InteractNode[interactables.Count];
for (int i = 0; i < interactables.Count; i++)
{
interactNodes[i] = new InteractNode();
interactNodes[i].NodeName = interactables[i].name;
interactNodes[i].Dimensions = interactables[i].bounds;
}
parts = meshes.ToArray();
}
