private void FourCollisionsButton_Click(object sender, EventArgs e)
{
CollisionAPI API = new CollisionAPI();
// Create some obstacles, at 4 corners of a square
List <CollisionShape> shapes = new List <CollisionShape>();
CollisionSphere sphere = new CollisionSphere(new Vector3(0f, 0f, 2f), 1f);
shapes.Add(sphere);
API.AddCollisionShape(sphere, 1);
CollisionCapsule capsule = new CollisionCapsule(new Vector3(10f, 0f, 0f),
new Vector3(10f, 0f, 4f),
1f);
shapes.Add(capsule);
API.AddCollisionShape(capsule, 2);
// Now, an AABB
CollisionAABB aabb = new CollisionAABB(new Vector3(9.5f, 9.5f, 0f),
new Vector3(10.5f, 10.5f, 4f));
shapes.Add(aabb);
API.AddCollisionShape(aabb, 3);
CollisionOBB obb = new CollisionOBB(new Vector3(0f, 10f, 2),
new Vector3[3] {
new Vector3(1f, 0f, 0f),
new Vector3(0f, 1f, 0f),
new Vector3(0f, 0f, 1f)
},
new Vector3(1f, 1f, 1f));
shapes.Add(obb);
API.AddCollisionShape(obb, 4);
FileStream f = new FileStream("C:\\Junk\\DumpSphereTree.txt", FileMode.Create, FileAccess.Write);
StreamWriter writer = new StreamWriter(f);
API.DumpSphereTree(writer);
API.SphereTreeRoot.VerifySphereTree();
// Now a moving object capsule in the center of the square
CollisionCapsule moCap = new CollisionCapsule(new Vector3(5f, 5f, 0f),
new Vector3(5f, 5f, 4f),
1f);
// Remember where the moving object started
Vector3 start = moCap.center;
// Move the moving object toward each of the obstacles
foreach (CollisionShape s in shapes)
{
moCap.AddDisplacement(start - moCap.center);
MoveToObject(writer, API, s, moCap);
}
writer.Close();
}
private static void TraceObstacle(CollisionShape obstacle)
{
if (obstacle is CollisionOBB)
{
CollisionOBB box = (CollisionOBB)obstacle;
MO.Log(" obstacle top {0} center {1} {2}",
box.center.y + box.extents[1], box.center, box);
}
else
{
MO.Log(" obstacle center {0} {1}", obstacle.center, obstacle);
}
}
public void ReadBox(string objectId, Matrix4 transform, XmlNode node, PhysicsData physicsData)
{
Quaternion rot = MathHelpers.GetRotation(transform);
Matrix4 tmpTransform = rot.Inverse().ToRotationMatrix() * transform;
Vector3 scaleDelta = tmpTransform.Scale - Vector3.UnitScale;
if (scaleDelta.Length > PhysicsSerializer.ScaleEpsilon)
{
log.Error("Scale is not currently supported for box shapes");
}
Vector3 halfExtents = Vector3.Zero;
foreach (XmlNode childNode in node.ChildNodes)
{
switch (childNode.Name)
{
case "half_extents":
ReadVector(ref halfExtents, childNode);
break;
default:
DebugMessage(childNode);
break;
}
}
Vector3 center = unitConversion * transform.Translation;
halfExtents *= unitConversion;
CollisionShape collisionShape;
float angle = 0;
Vector3 axis = Vector3.Zero;
rot.ToAngleAxis(ref angle, ref axis);
// I could build AABB shapes for boxes that are aligned, but then
// I can't drop instances in the world with arbitrary rotations.
// Instead, just always use an OBB.
//if (angle < PhysicsSerializer.RotateEpsilon) {
// collisionShape = new CollisionAABB(center - halfExtents, center + halfExtents);
//} else {
// TODO: I'm not sure I understand the OBB constructor
Vector3[] axes = new Vector3[3];
axes[0] = rot.XAxis;
axes[1] = rot.YAxis;
axes[2] = rot.ZAxis;
collisionShape = new CollisionOBB(center, axes, halfExtents);
physicsData.AddCollisionShape(objectId, collisionShape);
}
public XmlNode WriteShape(CollisionShape shape, XmlDocument document)
{
XmlNode node = document.CreateElement("shape");
if (shape is CollisionAABB)
{
CollisionAABB aabbShape = shape as CollisionAABB;
// convert to meters (or other scaled unit) before writing out
Vector3 center = shape.center / unitConversion;
Vector3 halfExtent = (aabbShape.max - aabbShape.center) / unitConversion;
XmlNode translate = WriteTranslate(center, document);
node.AppendChild(translate);
XmlNode boxNode = WriteBox(halfExtent, document);
node.AppendChild(boxNode);
}
else if (shape is CollisionOBB)
{
CollisionOBB obbShape = shape as CollisionOBB;
// convert to meters (or other scaled unit) before writing out
Vector3 center = shape.center / unitConversion;
Vector3 halfExtent = obbShape.extents / unitConversion;
Quaternion rot = Quaternion.Identity;
rot.FromAxes(obbShape.axes[0], obbShape.axes[1], obbShape.axes[2]);
XmlNode rotate = WriteRotate(rot, document);
node.AppendChild(rotate);
XmlNode translate = WriteTranslate(center, document);
node.AppendChild(translate);
XmlNode boxNode = WriteBox(halfExtent, document);
node.AppendChild(boxNode);
}
else if (shape is CollisionCapsule)
{
CollisionCapsule capsuleShape = shape as CollisionCapsule;
// convert to meters (or other scaled unit) before writing out
Vector3 center = shape.center / unitConversion;
float height = capsuleShape.height / unitConversion;
float radius = capsuleShape.capRadius / unitConversion;
Vector3 halfExtent = capsuleShape.topcenter - capsuleShape.center;
Quaternion rot = Vector3.UnitY.GetRotationTo(halfExtent);
XmlNode rotate = WriteRotate(rot, document);
node.AppendChild(rotate);
XmlNode translate = WriteTranslate(center, document);
node.AppendChild(translate);
XmlNode capsuleNode = WriteCapsule(height, radius, document);
node.AppendChild(capsuleNode);
}
else if (shape is CollisionSphere)
{
// convert to meters (or other scaled unit) before writing out
Vector3 center = shape.center / unitConversion;
float radius = shape.radius / unitConversion;
XmlNode translate = WriteTranslate(center, document);
node.AppendChild(translate);
XmlNode sphereNode = WriteSphere(radius, document);
node.AppendChild(sphereNode);
}
else
{
log.ErrorFormat("Unsupported collision shape: {0}", shape.GetType());
}
return(node);
}
// Take a different approach, based on an idea of Robin's:
// Find the farthest point pair, and use that to identify the
// triangles with one of the points as a vertex. Then take
// the normals of the triangles, adjust to object the
// right-hand rule, and they are the axes. Compute the center
// from the average of the farthest points, and extents by
// dotting farthest - center with the axes.
private static CollisionShape ExtractBox(SubMesh subMesh)
{
if (DoLog)
{
Log("");
Log(string.Format("Extracting box for submesh {0}", subMesh.Name));
}
MeshTriangle[] triangles = ExtractSubmeshTriangles(subMesh);
int count = triangles.Length;
// Find the two farthest vertices across all triangle
Vector3[] farthestPoints = new Vector3[2] {
Vector3.Zero, Vector3.Zero
};
float farthestDistanceSquared = 0.0f;
for (int i = 0; i < count; i++)
{
MeshTriangle t1 = triangles[i];
for (int j = 0; j < 3; j++)
{
Vector3 p1 = t1.vertPos[j];
for (int r = i; r < count; r++)
{
MeshTriangle t2 = triangles[r];
for (int s = 0; s < 3; s++)
{
Vector3 p2 = t2.vertPos[s];
Vector3 diff = (p1 - p2);
float d = diff.LengthSquared;
// if (DoLog)
// Log(string.Format(" TriVert {0} {1} {2} / {3} {4} {5} dist {6}", i, j, p1, r, s, p2, d));
if (d > farthestDistanceSquared)
{
if (DoLog)
{
Log(string.Format(" Largest! TriVert {0} {1} {2} / {3} {4} {5} dist {6}",
i, j, p1, r, s, p2, d));
}
farthestDistanceSquared = d;
farthestPoints[0] = p1;
farthestPoints[1] = p2;
}
}
}
}
}
// The center is the average of the farthest points
Vector3 center = (farthestPoints[0] + farthestPoints[1]) * 0.5f;
if (DoLog)
{
Log(string.Format("The farthest points are {0} and {1}",
farthestPoints[0], farthestPoints[1]));
Log(string.Format("The center is {0}", center));
}
// Now find the three triangles that have the
// farthestPoints[0] as a vertex
Vector3[] axes = new Vector3[] { Vector3.Zero, Vector3.Zero, Vector3.Zero };
int foundCount = 0;
for (int i = 0; i < count; i++)
{
MeshTriangle t = triangles[i];
for (int j = 0; j < 3; j++)
{
Vector3 p = t.vertPos[j];
if ((p - farthestPoints[0]).LengthSquared < geometryEpsilon)
{
Vector3 side1 = t.vertPos[1] - t.vertPos[0];
Vector3 side2 = t.vertPos[2] - t.vertPos[1];
Vector3 axis = side1.Cross(side2).ToNormalized();
// Ignore this triangle if his normal matches one we already have
bool ignore = false;
for (int k = 0; k < foundCount; k++)
{
if (Math.Abs(axis.Cross(axes[k]).LengthSquared) < geometryEpsilon)
{
ignore = true;
break;
}
}
if (!ignore)
{
Debug.Assert(foundCount < 3, "Found more than three triangles with distinct normals and vertex = farthest point");
axes[foundCount] = axis;
foundCount++;
}
}
}
}
// Put the axes in coordinate order
for (int i = 0; i < 3; i++)
{
float largest = float.MinValue;
int largestIndex = i;
for (int j = 0; j < 3; j++)
{
float v = Math.Abs(axes[j][i]);
if (v > largest)
{
largestIndex = j;
largest = v;
}
}
if (largestIndex != i)
{
Vector3 t = axes[i];
axes[i] = axes[largestIndex];
axes[largestIndex] = t;
}
if (axes[i][i] < 0)
{
axes[i] = -axes[i];
}
}
// Put the axes in right-hand-rule order
if (axes[0].Cross(axes[1]).Dot(axes[2]) < 0)
{
axes[2] = -axes[2];
}
Debug.Assert(axes[0].Cross(axes[1]).Dot(axes[2]) > 0,
"After swapping axes, still don't obey right-hand rule");
// The extents are just the abs of the dot products of
// farthest point minus the center with the axes
Vector3 f = farthestPoints[0] - center;
Vector3 extents = new Vector3(Math.Abs(f.Dot(axes[0])),
Math.Abs(f.Dot(axes[1])),
Math.Abs(f.Dot(axes[2])));
if (DoLog)
{
for (int i = 0; i < 3; i++)
{
Log(string.Format(" axis[{0}] {1}, extent[{2}] {3}", i, axes[i], i, extents[i]));
}
int[] sign = new int[3] {
0, 0, 0
};
for (int i = -1; i < 2; i += 2)
{
sign[0] = i;
for (int j = -1; j < 2; j += 2)
{
sign[1] = j;
for (int k = -1; k < 2; k += 2)
{
sign[2] = k;
Vector3 corner = center;
for (int a = 0; a < 3; a++)
{
corner += axes[a] * extents[a] * sign[a];
}
Log(string.Format(" corner[{0},{1},{2}] = {3}", i, j, k, corner));
}
}
}
}
// Now, is it an obb or an aabb? Figure out if the axes
// point in the same direction as the basis vectors, and
// if so, the order of the axes
int[] mapping = new int[3] {
-1, -1, -1
};
int foundMapping = 0;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
if (Math.Abs(axes[i].Dot(Primitives.UnitBasisVectors[j]) - 1.0f) < .0001f)
{
if (DoLog)
{
Log(string.Format(" foundMapping[{0}], basis vector {1}", i, Primitives.UnitBasisVectors[j]));
}
mapping[i] = j;
foundMapping++;
break;
}
}
}
CollisionShape shape;
if (foundMapping == 3)
{
// It's an AABB, so build the min and max vectors, in
// the order that matches the unit basis vector order
Vector3 min = Vector3.Zero;
Vector3 max = Vector3.Zero;
for (int i = 0; i < 3; i++)
{
float e = extents[i];
int j = mapping[i];
min[j] = center[j] - e;
max[j] = center[j] + e;
}
shape = new CollisionAABB(min, max);
}
else
{
// Return the OBB
shape = new CollisionOBB(center, axes, extents);
}
if (DoLog)
{
Log(string.Format("Extraction result: {0}", shape));
}
return(shape);
}
private static CollisionShape ExtractBox_Old(SubMesh subMesh)
{
if (DoLog)
{
Log("");
Log(string.Format("Extracting box for submesh {0}", subMesh.Name));
}
MeshTriangle[] triangles = ExtractSubmeshTriangles(subMesh);
int count = triangles.Length;
Plane[] planesUnsorted = new Plane[6];
int planeCount = 0;
// Iterate through the triangles. For each triangle,
// determine the plane it belongs to, and find the plane
// in the array of planes, or if it's not found, add it.
for (int i = 0; i < count; i++)
{
MeshTriangle t = triangles[i];
bool found = false;
Plane p = new Plane(t.vertPos[0], t.vertPos[1], t.vertPos[2]);
NormalizePlane(ref p);
if (DoLog)
{
Log(string.Format(" {0} => plane {1}", t.ToString(), p.ToString()));
}
for (int j = 0; j < planeCount; j++)
{
Plane pj = planesUnsorted[j];
if (SamePlane(pj, p))
{
if (DoLog)
{
Log(string.Format(" plane {0} same as plane {1}", p.ToString(), pj.ToString()));
}
found = true;
break;
}
}
if (!found)
{
if (planeCount < 6)
{
if (DoLog)
{
Log(string.Format(" plane[{0}] = plane {1}", planeCount, p.ToString()));
}
planesUnsorted[planeCount++] = p;
}
else
{
Debug.Assert(false,
string.Format("In the definition of box {0}, more than 6 planes were found",
subMesh.Name));
}
}
}
Debug.Assert(planeCount == 6,
string.Format("In the definition of box {0}, fewer than 6 planes were found",
subMesh.Name));
// Now recreate the planes array, making sure that
// opposite faces are 3 planes apart
Plane[] planes = new Plane[6];
bool[] planeUsed = new bool[6];
for (int i = 0; i < 6; i++)
{
planeUsed[i] = false;
}
int planePair = 0;
for (int i = 0; i < 6; i++)
{
if (!planeUsed[i])
{
Plane p1 = planesUnsorted[i];
planes[planePair] = p1;
planeUsed[i] = true;
for (int j = 0; j < 6; j++)
{
Plane p2 = planesUnsorted[j];
if (!planeUsed[j] && !Orthogonal(p2, p1))
{
planes[3 + planePair++] = p2;
planeUsed[j] = true;
break;
}
}
}
}
Debug.Assert(planePair == 3, "Didn't find 3 pairs of parallel planes");
// Make sure that the sequence of planes follows the
// right-hand rule
if (planes[0].Normal.Cross(planes[1].Normal).Dot(planes[3].Normal) < 0)
{
// Swap the first two plane pairs
Plane p = planes[0];
planes[0] = planes[1];
planes[1] = p;
p = planes[0 + 3];
planes[0 + 3] = planes[1 + 3];
planes[1 + 3] = p;
Debug.Assert(planes[0].Normal.Cross(planes[1].Normal).Dot(planes[3].Normal) > 0,
"Even after swapping, planes don't obey the right-hand rule");
}
// Now we have our 6 planes, sorted so that opposite
// planes are 3 planes apart, and so they obey the
// right-hand rule. This guarantees that corners
// correspond. Find the 8 intersections that define the
// corners.
Vector3[] corners = new Vector3[8];
int cornerCount = 0;
for (int i = 0; i <= 3; i += 3)
{
Plane p1 = planes[i];
for (int j = 1; j <= 4; j += 3)
{
Plane p2 = planes[j];
for (int k = 2; k <= 5; k += 3)
{
Plane p3 = planes[k];
Vector3 corner = -1 * ((p1.D * (p2.Normal.Cross(p3.Normal)) +
p2.D * (p3.Normal.Cross(p1.Normal)) +
p3.D * (p1.Normal.Cross(p2.Normal))) /
p1.Normal.Dot(p2.Normal.Cross(p3.Normal)));
Debug.Assert(cornerCount < 8,
string.Format("In the definition of box {0}, more than 8 corners were found",
subMesh.Name));
if (DoLog)
{
Log(string.Format(" corner#{0}: {1}", cornerCount, corner.ToString()));
}
corners[cornerCount++] = corner;
}
}
}
Debug.Assert(cornerCount == 8,
string.Format("In the definition of box {0}, fewer than 8 corners were found",
subMesh.Name));
// We know that corners correspond. Now find the center
Vector3 center = (corners[0] + corners[7]) / 2;
Debug.Assert((center - (corners[1] + corners[5]) / 2.0f).Length > geometryEpsilon ||
(center - (corners[2] + corners[6]) / 2.0f).Length > geometryEpsilon ||
(center - (corners[3] + corners[7]) / 2.0f).Length > geometryEpsilon,
string.Format("In the definition of box {0}, center definition {0} is not consistent",
subMesh.Name, center.ToString()));
// Find the extents
Vector3 extents = new Vector3(Math.Abs((corners[1] - corners[0]).Length / 2.0f),
Math.Abs((corners[3] - corners[1]).Length / 2.0f),
Math.Abs((corners[4] - corners[0]).Length / 2.0f));
if (DoLog)
{
Log(string.Format(" extents {0}", extents.ToString()));
}
// Find the axes
Vector3[] axes = new Vector3[3] {
(corners[1] - corners[0]).ToNormalized(),
(corners[3] - corners[1]).ToNormalized(),
(corners[4] - corners[0]).ToNormalized()
};
if (DoLog)
{
for (int i = 0; i < 3; i++)
{
Log(string.Format(" axis[{0}] {1}", i, axes[i]));
}
}
// Now, is it an obb or an aabb? Figure out if the axes
// point in the same direction as the basis vectors, and
// if so, the order of the axes
int[] mapping = new int[3] {
-1, -1, -1
};
int foundMapping = 0;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
if (axes[i].Cross(Primitives.UnitBasisVectors[j]).Length < geometryEpsilon)
{
mapping[i] = j;
foundMapping++;
break;
}
}
}
CollisionShape shape;
if (foundMapping == 3)
{
// It's an AABB, so build the min and max vectors, in
// the order that matches the unit basis vector order
Vector3 min = Vector3.Zero;
Vector3 max = Vector3.Zero;
for (int i = 0; i < 3; i++)
{
float e = extents[i];
int j = mapping[i];
min[j] = center[j] - e;
max[j] = center[j] + e;
}
shape = new CollisionAABB(min, max);
}
else
{
Vector3 ruleTest = axes[0].Cross(axes[1]);
if (axes[2].Dot(ruleTest) < 0)
{
axes[2] = -1 * axes[2];
}
// Return the OBB
shape = new CollisionOBB(center, axes, extents);
}
if (DoLog)
{
Log(string.Format("Extraction result: {0}", shape));
}
return(shape);
}
