// Closest point tests
public static Vector3 closestPointToOBB(Vector3 p, OrientedBox obb)
{
Vector3 q = new Vector3();
float v;
// Transform point into OBB local coordinates so we can treat OBB as an AABB
Vector3 transformedP = Utility.Multiply(p, Matrix.Invert(obb.transform));
// Closest point on AABB to point
v = transformedP.X;
if (v < obb.min.X) v = obb.min.X;
if (v > obb.max.X) v = obb.max.X;
q.X = v;
v = transformedP.Y;
if (v < obb.min.Y) v = obb.min.Y;
if (v > obb.max.Y) v = obb.max.Y;
q.Y = v;
v = transformedP.Z;
if (v < obb.min.Z) v = obb.min.Z;
if (v > obb.max.Z) v = obb.max.Z;
q.Z = v;
// Transform closest point in world coordinates
q = Utility.Multiply(q, obb.transform);
return q;
}
public OrientedBox(OrientedBox o)
: base()
{
this.type = Type.OBB;
center = o.center;
extent = o.extent;
transform = o.transform;
min = o.min;
max = o.max;
}
public static bool getAABBOBBCollision(Shape s1, Shape s2)
{
AxisAlignedBox aabb;
OrientedBox obb;
// Safe type conversion
if (s1.type == Shape.Type.AABB)
{
aabb = (AxisAlignedBox)s1;
obb = (OrientedBox)s2;
}
else
{
aabb = (AxisAlignedBox)s2;
obb = (OrientedBox)s1;
}
OrientedBox o2 = new OrientedBox(aabb.min, aabb.max);
return getOBBOBBCollision(obb, o2);
}
public static void initShapeCatalog(string rootDir)
{
// Create object shapes
shapes = new Dictionary<string, Shape>();
// Load xml file
XDocument doc = XDocument.Load(rootDir + "\\XML\\shapes.xml");
foreach (XElement node in doc.Element("shapes").Descendants("shape"))
{
string name = node.Attribute("name").Value;
string type = node.Attribute("type").Value;
if (type == "aabb")
{
Vector3 min = new Vector3((float)XmlConvert.ToDouble(node.Element("min").Attribute("x").Value),
(float)XmlConvert.ToDouble(node.Element("min").Attribute("y").Value),
(float)XmlConvert.ToDouble(node.Element("min").Attribute("z").Value));
Vector3 max = new Vector3((float)XmlConvert.ToDouble(node.Element("max").Attribute("x").Value),
(float)XmlConvert.ToDouble(node.Element("max").Attribute("y").Value),
(float)XmlConvert.ToDouble(node.Element("max").Attribute("z").Value));
shapes[name] = new AxisAlignedBox(min, max);
}
else if (type == "obb")
{
Vector3 min = new Vector3((float)XmlConvert.ToDouble(node.Element("min").Attribute("x").Value),
(float)XmlConvert.ToDouble(node.Element("min").Attribute("y").Value),
(float)XmlConvert.ToDouble(node.Element("min").Attribute("z").Value));
Vector3 max = new Vector3((float)XmlConvert.ToDouble(node.Element("max").Attribute("x").Value),
(float)XmlConvert.ToDouble(node.Element("max").Attribute("y").Value),
(float)XmlConvert.ToDouble(node.Element("max").Attribute("z").Value));
shapes[name] = new OrientedBox(min, max);
}
else if (type == "sphere")
{
Vector3 center = new Vector3((float)XmlConvert.ToDouble(node.Element("center").Attribute("x").Value),
(float)XmlConvert.ToDouble(node.Element("center").Attribute("y").Value),
(float)XmlConvert.ToDouble(node.Element("center").Attribute("z").Value));
float radius = (float)XmlConvert.ToDouble(node.Element("radius").Attribute("value").Value);
shapes[name] = new Sphere(center, radius);
}
else
Log.log(Log.Type.WARNING, "Unknown shape type-> name: " + name + " type: " + type);
}
}
public static bool getOBBOBBCollision(Shape s1, Shape s2)
{
OrientedBox o1 = (OrientedBox)s1;
OrientedBox o2 = (OrientedBox)s2;
// Matrix to transform other OBB into my reference to allow me to be treated as an AABB
Matrix toMe = o2.transform * Matrix.Invert(o1.transform);
Vector3 centerOther = Utility.Multiply(o2.center, toMe);
Vector3 extentsOther = o2.extent;
Vector3 separation = centerOther - o1.center;
Matrix3 rotations = new Matrix3(toMe);
Matrix3 absRotations = Utility.Abs(rotations);
float r, r0, r1, r01;
//--- Test case 1 - X axis
r = Math.Abs(separation.X);
r1 = Vector3.Dot(extentsOther, absRotations.Column(0));
r01 = o1.extent.X + r1;
if (r > r01)
{
return(false);
}
//--- Test case 1 - Y axis
r = Math.Abs(separation.Y);
r1 = Vector3.Dot(extentsOther, absRotations.Column(1));
r01 = o1.extent.Y + r1;
if (r > r01)
{
return(false);
}
//--- Test case 1 - Z axis
r = Math.Abs(separation.Z);
r1 = Vector3.Dot(extentsOther, absRotations.Column(2));
r01 = o1.extent.Z + r1;
if (r > r01)
{
return(false);
}
//--- Test case 2 - X axis
r = Math.Abs(Vector3.Dot(rotations.Row(0), separation));
r0 = Vector3.Dot(o1.extent, absRotations.Row(0));
r01 = r0 + extentsOther.X;
if (r > r01)
{
return(false);
}
//--- Test case 2 - Y axis
r = Math.Abs(Vector3.Dot(rotations.Row(1), separation));
r0 = Vector3.Dot(o1.extent, absRotations.Row(1));
r01 = r0 + extentsOther.Y;
if (r > r01)
{
return(false);
}
//--- Test case 2 - Z axis
r = Math.Abs(Vector3.Dot(rotations.Row(2), separation));
r0 = Vector3.Dot(o1.extent, absRotations.Row(2));
r01 = r0 + extentsOther.Z;
if (r > r01)
{
return(false);
}
//--- Test case 3 # 1
r = Math.Abs(separation.Z * rotations[0, 1] - separation.Y * rotations[0, 2]);
r0 = o1.extent.Y * absRotations[0, 2] + o1.extent.Z * absRotations[0, 1];
r1 = extentsOther.Y * absRotations[2, 0] + extentsOther.Z * absRotations[1, 0];
r01 = r0 + r1;
if (r > r01)
{
return(false);
}
//--- Test case 3 # 2
r = Math.Abs(separation.Z * rotations[1, 1] - separation.Y * rotations[1, 2]);
r0 = o1.extent.Y * absRotations[1, 2] + o1.extent.Z * absRotations[1, 1];
r1 = extentsOther.X * absRotations[2, 0] + extentsOther.Z * absRotations[0, 0];
r01 = r0 + r1;
if (r > r01)
{
return(false);
}
//--- Test case 3 # 3
r = Math.Abs(separation.Z * rotations[2, 1] - separation.Y * rotations[2, 2]);
r0 = o1.extent.Y * absRotations[2, 2] + o1.extent.Z * absRotations[2, 1];
r1 = extentsOther.X * absRotations[1, 0] + extentsOther.Y * absRotations[0, 0];
r01 = r0 + r1;
if (r > r01)
{
return(false);
}
//--- Test case 3 # 4
r = Math.Abs(separation.X * rotations[0, 2] - separation.Z * rotations[0, 0]);
r0 = o1.extent.X * absRotations[0, 2] + o1.extent.Z * absRotations[0, 0];
r1 = extentsOther.Y * absRotations[2, 1] + extentsOther.Z * absRotations[1, 1];
r01 = r0 + r1;
if (r > r01)
{
return(false);
}
//--- Test case 3 # 5
r = Math.Abs(separation.X * rotations[1, 2] - separation.Z * rotations[1, 0]);
r0 = o1.extent.X * absRotations[1, 2] + o1.extent.Z * absRotations[1, 0];
r1 = extentsOther.X * absRotations[2, 1] + extentsOther.Z * absRotations[0, 1];
r01 = r0 + r1;
if (r > r01)
{
return(false);
}
//--- Test case 3 # 6
r = Math.Abs(separation.X * rotations[2, 2] - separation.Z * rotations[2, 0]);
r0 = o1.extent.X * absRotations[2, 2] + o1.extent.Z * absRotations[2, 0];
r1 = extentsOther.X * absRotations[1, 1] + extentsOther.Y * absRotations[0, 1];
r01 = r0 + r1;
if (r > r01)
{
return(false);
}
//--- Test case 3 # 7
r = Math.Abs(separation.Y * rotations[0, 0] - separation.X * rotations[0, 1]);
r0 = o1.extent.X * absRotations[0, 1] + o1.extent.Y * absRotations[0, 0];
r1 = extentsOther.Y * absRotations[2, 2] + extentsOther.Z * absRotations[1, 2];
r01 = r0 + r1;
if (r > r01)
{
return(false);
}
//--- Test case 3 # 8
r = Math.Abs(separation.Y * rotations[1, 0] - separation.X * rotations[1, 1]);
r0 = o1.extent.X * absRotations[1, 1] + o1.extent.Y * absRotations[1, 0];
r1 = extentsOther.X * absRotations[2, 2] + extentsOther.Z * absRotations[0, 2];
r01 = r0 + r1;
if (r > r01)
{
return(false);
}
//--- Test case 3 # 9
r = Math.Abs(separation.Y * rotations[2, 0] - separation.X * rotations[2, 1]);
r0 = o1.extent.X * absRotations[2, 1] + o1.extent.Y * absRotations[2, 0];
r1 = extentsOther.X * absRotations[1, 2] + extentsOther.Y * absRotations[0, 2];
r01 = r0 + r1;
if (r > r01)
{
return(false);
}
return(true); // No separating axis, then we have intersection
}