// 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 }