private static RenderedNode NewRenderedBox(int id, int index, CollisionOBB box) { RenderedNode n = UnscaledRenderedObject("unit_box.mesh", id, index, box.center); n.node.ScaleFactor = 2 * box.extents * MO.DivMeter; Quaternion q = Quaternion.Identity; q.FromAxes(box.axes[0], box.axes[1], box.axes[2]); n.node.Orientation = q; return(n); }
public override CollisionShape Clone() { Vector3[] newAxes = new Vector3[3]; newAxes[0] = axes[0]; newAxes[1] = axes[1]; newAxes[2] = axes[2]; CollisionOBB rv = new CollisionOBB(center, newAxes, extents); rv.handle = this.handle; rv.timeStamp = this.timeStamp; return(rv); }
public static void CaseNoZeros(ref Vector3 rkPnt, Vector3 rkDir, CollisionOBB rkBox, SegOBBParams pf, ref float rfSqrDistance) { Vector3 kPmE = new Vector3(rkPnt.x - rkBox.extents[0], rkPnt.y - rkBox.extents[1], rkPnt.z - rkBox.extents[2]); float fProdDxPy, fProdDyPx, fProdDzPx, fProdDxPz, fProdDzPy, fProdDyPz; fProdDxPy = rkDir.x*kPmE.y; fProdDyPx = rkDir.y*kPmE.x; if (fProdDyPx >= fProdDxPy) { fProdDzPx = rkDir.z*kPmE.x; fProdDxPz = rkDir.x*kPmE.z; if (fProdDzPx >= fProdDxPz) { // line intersects x = e0 Face(0,1,2,ref rkPnt,rkDir,rkBox,kPmE,pf,ref rfSqrDistance); } else { // line intersects z = e2 Face(2,0,1,ref rkPnt,rkDir,rkBox,kPmE,pf,ref rfSqrDistance); } } else { fProdDzPy = rkDir.z*kPmE.y; fProdDyPz = rkDir.y*kPmE.z; if (fProdDzPy >= fProdDyPz) { // line intersects y = e1 Face(1,2,0,ref rkPnt,rkDir,rkBox,kPmE,pf,ref rfSqrDistance); } else { // line intersects z = e2 Face(2,0,1,ref rkPnt,rkDir,rkBox,kPmE,pf,ref rfSqrDistance); } } }
public static void Case000(ref Vector3 rkPnt, CollisionOBB rkBox, ref float rfSqrDistance) { float fDelta; for (int i=0; i<3; i++) { if (rkPnt[i] < -rkBox.extents[i]) { fDelta = rkPnt[i] + rkBox.extents[i]; rfSqrDistance += fDelta*fDelta; rkPnt[i] = -rkBox.extents[i]; } else if (rkPnt[i] > rkBox.extents[i]) { fDelta = rkPnt[i] - rkBox.extents[i]; rfSqrDistance += fDelta*fDelta; rkPnt[i] = rkBox.extents[i]; } } }
public static bool TestOBBOBB(CollisionOBB a, CollisionOBB b, CollisionParms parms) { if (MO.DoLog) MO.Log("TestOBBOBB Entering: a {0} b {1}", a, b); if (TestOBBOBBInternal(a, b)) { Vector3 n = b.center - a.center; // ??? Not right parms.SetNormPart(n); parms.SetNormObstacle(-n); if (MO.DoLog) MO.Log("TestOBBOBB Collided: n {0}", n); return true; } else return false; }
public static bool TestCapsuleOBB(CollisionCapsule a, CollisionOBB b, CollisionParms parms) { SegOBBParams obbParms = new SegOBBParams(true); Segment s = Segment.SegmentFromStartAndEnd(a.bottomcenter, a.topcenter); if (SqrDistSegOBB(s, b, obbParms) < a.capRadius * a.capRadius) { // If parms.pfLParam == 0, closest is bottomcenter; if == 1, // closest is topcenter. Vector3 d = a.bottomcenter + obbParms.pfLParam * s.direction; // pfBVec is relative to the center of the box, but in box // coords. Vector3 f = b.center; for (int i=0; i<3; i++) f += obbParms.pfBVec[i] * b.axes[i]; Vector3 n = f - d; parms.SetNormPart(n); parms.SetNormObstacle(-n); if (MO.DoLog) { MO.Log(" TestCapsuleOBB: pfLParam {0}, pfBVec {1}", obbParms.pfLParam, obbParms.pfBVec); MO.Log(" TestCapsuleOBB: d {0}, f {1}", f, d); MO.Log(" -n {0}", -n); } return true; } else return false; }
public static float SqrDistSegOBBInternal(Segment rkLine, CollisionOBB rkBox, SegOBBParams pf) { // compute coordinates of line in box coordinate system Vector3 kDiff = rkLine.origin - rkBox.center; Vector3 kPnt = new Vector3(kDiff.Dot(rkBox.axes[0]), kDiff.Dot(rkBox.axes[1]), kDiff.Dot(rkBox.axes[2])); Vector3 kDir = new Vector3(rkLine.direction.Dot(rkBox.axes[0]), rkLine.direction.Dot(rkBox.axes[1]), rkLine.direction.Dot(rkBox.axes[2])); // Apply reflections so that direction vector has nonnegative components. bool[] bReflect = new bool[3] { false, false, false }; int i; for (i = 0; i < 3; i++) { if (kDir[i] < 0.0f) { kPnt[i] = -kPnt[i]; kDir[i] = -kDir[i]; bReflect[i] = true; } } float fSqrDistance = 0.0f; if (kDir.x > 0.0f) { if (kDir.y > 0.0f) { if (kDir.z > 0.0f) { // (+,+,+) CaseNoZeros(ref kPnt,kDir,rkBox,pf,ref fSqrDistance); } else { // (+,+,0) Case0(0,1,2,ref kPnt,kDir,rkBox,pf,ref fSqrDistance); } } else { if (kDir.z > 0.0f) { // (+,0,+) Case0(0,2,1,ref kPnt,kDir,rkBox,pf,ref fSqrDistance); } else { // (+,0,0) Case00(0,1,2,ref kPnt,kDir,rkBox,pf,ref fSqrDistance); } } } else { if (kDir.y > 0.0f) { if (kDir.z > 0.0f) { // (0,+,+) Case0(1,2,0,ref kPnt,kDir,rkBox,pf,ref fSqrDistance); } else { // (0,+,0) Case00(1,0,2,ref kPnt,kDir,rkBox,pf,ref fSqrDistance); } } else { if (kDir.z > 0.0f) { // (0,0,+) Case00(2,0,1,ref kPnt,kDir,rkBox,pf,ref fSqrDistance); } else { // (0,0,0) Case000(ref kPnt,rkBox,ref fSqrDistance); if (pf.useIt) pf.pfLParam = 0.0f; } } } if (pf.useIt) { // undo reflections for (i = 0; i < 3; i++) { if (bReflect[i]) kPnt[i] = -kPnt[i]; } pf.pfBVec = kPnt; } return fSqrDistance; }
// Returns square of the distance from a point to an OBB public static float SqDistPointOBB(Vector3 p, CollisionOBB b, out Vector3 c) { ClosestPtPointOBB(p, b, out c); Vector3 v = p - c; return v.Dot(v); }
private static void TestPhysics( string srcDir, string dstDir, string name ) { float OneMeter = 1000; PhysicsData pd = new PhysicsData(); Vector3 center = new Vector3( 10 * OneMeter, 1 * OneMeter, 1 * OneMeter ); Vector3[] obbAxes = new Vector3[ 3 ]; obbAxes[ 0 ] = new Vector3( 1, 0, -1 ); obbAxes[ 0 ].Normalize(); obbAxes[ 1 ] = Vector3.UnitY; obbAxes[ 2 ] = new Vector3( 1, 0, 1 ); obbAxes[ 2 ].Normalize(); Vector3 obbExtents = new Vector3( 2.5f * OneMeter, 1 * OneMeter, 1 * OneMeter ); CollisionOBB obb = new CollisionOBB( center, obbAxes, obbExtents ); pd.AddCollisionShape( "submesh01", obb ); CollisionAABB aabb = new CollisionAABB( center - obbExtents, center + obbExtents ); pd.AddCollisionShape( "submesh01", aabb ); CollisionSphere sphere = new CollisionSphere( center, 2 * OneMeter ); pd.AddCollisionShape( "submesh01", sphere ); Vector3 capExtents = new Vector3( -1 * OneMeter, 0, 0 ); CollisionCapsule capsule = new CollisionCapsule( center + capExtents, center - capExtents, .5f * OneMeter ); pd.AddCollisionShape( "submesh01", capsule ); PhysicsSerializer ps = new PhysicsSerializer(); ps.ExportPhysics( pd, name ); pd = new PhysicsData(); ps.ImportPhysics( pd, name ); foreach( string objectId in pd.GetCollisionObjects() ) { log.InfoFormat( "Collision shapes for: {0}", objectId ); List<CollisionShape> collisionShapes = pd.GetCollisionShapes( objectId ); foreach( CollisionShape shape in collisionShapes ) log.InfoFormat( "Shape: {0}", shape ); } }
// Keep the code below, commented out, because it is useful // for debugging near plane issues // private static int nearPlaneCollisionCount = 0; // private static int nearPlaneNodeId = 99999; // private static List<RenderedNode>[] nearPlaneRenderedNodes = new List<RenderedNode>[4] { // new List<RenderedNode>(), new List<RenderedNode>(), new List<RenderedNode>(), new List<RenderedNode>() }; // private static CollisionCapsule[] nearPlaneCapsules = new CollisionCapsule[4]; // // protected void CreateNearPlaneCapsules(Camera camera, // Vector3 cameraPosition, // Vector3 playerHeadPosition) // { // // Frame the clip plane with 4 narrow capsules // // Start by finding the center of the near plane // Vector3 toPlayer = (playerHeadPosition - cameraPosition).ToNormalized(); // Vector3 center = cameraPosition + (1.02f * camera.Near) * toPlayer; // float thetaY = MathUtil.DegreesToRadians(camera.FOVy * 0.5f); // float tanThetaY = MathUtil.Tan(thetaY); // float tanThetaX = tanThetaY * camera.AspectRatio; // Vector3 right = tanThetaX * camera.Near * (camera.Right.ToNormalized()); // Vector3 up = tanThetaY * camera.Near * (camera.Up.ToNormalized()); // Vector3 corner1 = center + right - up; // Vector3 corner2 = center + right + up; // Vector3 corner3 = center - right + up; // Vector3 corner4 = center - right - up; // nearPlaneCapsules[0] = new CollisionCapsule(corner1, corner2, Client.OneMeter * .001f); // nearPlaneCapsules[1] = new CollisionCapsule(corner2, corner3, Client.OneMeter * .001f); // nearPlaneCapsules[2] = new CollisionCapsule(corner3, corner4, Client.OneMeter * .001f); // nearPlaneCapsules[3] = new CollisionCapsule(corner4, corner1, Client.OneMeter * .001f); // // RenderedNode.Scene = sceneManager; // for (int i=0; i<4; i++) { // RenderedNode.RemoveNodeRendering(nearPlaneNodeId + i, ref nearPlaneRenderedNodes[i]); // RenderedNode.CreateRenderedNodes(nearPlaneCapsules[i], nearPlaneNodeId + i, ref nearPlaneRenderedNodes[i]); // } // } protected float FindAcceptableCameraPosition(Camera camera, Vector3 cameraPos, Vector3 cameraTarget) { //Logger.Log(0, "FindAcceptableCameraPosition cameraPos {0}, cameraTarget {1}", cameraPos, cameraTarget); // If there is no CollisionAPI object, punt CollisionAPI api = client.WorldManager.CollisionHelper; if (api == null) return (cameraPos - cameraTarget).Length; // Start by finding the point at which a narrow capsule // from the camera to the player intersects. If it // doesn't, then set the point to the cameraPos. // Make a very narrow capsule between the camera and the player's head CollisionCapsule cap = new CollisionCapsule(cameraPos, cameraTarget, Client.OneMeter * .001f); // 1 millimeter Vector3 newCameraPos = cameraPos; Vector3 unitTowardCamera = (newCameraPos - cameraTarget).ToNormalized(); // Test for a collision, setting intersection if there is one. Vector3 intersection = Vector3.Zero; if (api.FindClosestCollision(cap, cameraTarget, cameraPos, ref intersection) != null) { // There is A collision - - set the new camera pos to // be the point of intersection newCameraPos = intersection - unitTowardCamera * camera.Near; //Logger.Log(0, "FindAcceptableCameraPosition: Thin cap collides at {0}", newCameraPos); } // Move a near-plane OBB forward from the point of // intersection until we find a place where the near plane // doesn't intersect any collision volumes Vector3 center = newCameraPos - camera.Near * unitTowardCamera; float thetaY = MathUtil.DegreesToRadians(camera.FOVy * 0.5f); float tanThetaY = MathUtil.Tan(thetaY); float tanThetaX = tanThetaY * camera.AspectRatio; // Compute the OBB axes and extents Vector3[] axes = new Vector3[3]; Vector3 extents = Vector3.Zero; // The axis perpendicular to the near plane float obbDepth = 100f; // 100 mm thick axes[0] = -unitTowardCamera; axes[1] = -axes[0].Cross(Vector3.UnitY).ToNormalized(); axes[2] = -axes[1].Cross(axes[0]).ToNormalized(); extents[0] = obbDepth; extents[1] = camera.Near * tanThetaX; extents[2] = camera.Near * tanThetaY; float len = (newCameraPos - cameraTarget).Length; float startingLen = len; CollisionOBB obb = new CollisionOBB(center, axes, extents); CollisionParms parms = new CollisionParms(); while (len >= minThirdPersonDistance) { obb.center = cameraTarget + unitTowardCamera * len; bool collides = api.ShapeCollides(obb, parms); //Logger.Log(0, "FindAcceptableCameraPosition len {0}, obb center {1}, collides {2}", len, obb.center, collides); if (!collides) break; //client.Write(string.Format("OBB collides; len {0}", len)); len -= obbDepth; } // Len is less than the camera min distance, so just // return it //Logger.Log(0, "FindAcceptableCameraPosition: starting len {0} final len {1}", startingLen, len); return len; }
public override CollisionShape Clone() { Vector3[] newAxes = new Vector3[3]; newAxes[0] = axes[0]; newAxes[1] = axes[1]; newAxes[2] = axes[2]; CollisionOBB rv = new CollisionOBB(center, newAxes, extents); rv.handle = this.handle; rv.timeStamp = this.timeStamp; return rv; }
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); }
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; }
// 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; }
// Given point p, return point q on the surface of OBB b, closest to p public static void ClosestPtPointOBB(Vector3 p, CollisionOBB b, out Vector3 q) { Vector3 d = p - b.center; // Make sure P is on the surface of the OBB float pSquared = d.Dot(d); float bSquared = b.radius * b.radius; if (pSquared < bSquared) { p = p * (float)Math.Sqrt(bSquared / pSquared); } // Start result at center of box; make steps from there q = b.center; // For each OBB axis... for (int i = 0; i < 3; i++) { // ...project d onto that axis to get the distance // along the axis of d from the box center float dist = d.Dot(b.axes[i]); // If distance farther than the box extents, clamp to the box float e = b.extents[i]; dist = Clamp(dist, -e, e); // Step that distance along the axis to get world coordinate q += dist * b.axes[i]; } }
public static void Face(int i0, int i1, int i2, ref Vector3 rkPnt, Vector3 rkDir, CollisionOBB rkBox, Vector3 rkPmE, SegOBBParams pf, ref float rfSqrDistance) { Vector3 kPpE = Vector3.Zero; float fLSqr, fInv, fTmp, fParam, fT, fDelta; kPpE[i1] = rkPnt[i1] + rkBox.extents[i1]; kPpE[i2] = rkPnt[i2] + rkBox.extents[i2]; if (rkDir[i0]*kPpE[i1] >= rkDir[i1]*rkPmE[i0]) { if (rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0]) { // v[i1] >= -e[i1], v[i2] >= -e[i2] (distance = 0) if (pf.useIt) { rkPnt[i0] = rkBox.extents[i0]; fInv = 1.0f/rkDir[i0]; rkPnt[i1] -= rkDir[i1]*rkPmE[i0]*fInv; rkPnt[i2] -= rkDir[i2]*rkPmE[i0]*fInv; pf.pfLParam = -rkPmE[i0]*fInv; } } else { // v[i1] >= -e[i1], v[i2] < -e[i2] fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i2]*rkDir[i2]; fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] + rkDir[i2]*kPpE[i2]); if (fTmp <= 2.0*fLSqr*rkBox.extents[i1]) { fT = fTmp/fLSqr; fLSqr += rkDir[i1]*rkDir[i1]; fTmp = kPpE[i1] - fT; fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp + rkDir[i2]*kPpE[i2]; fParam = -fDelta/fLSqr; rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp + kPpE[i2]*kPpE[i2] + fDelta*fParam; if (pf.useIt) { pf.pfLParam = fParam; rkPnt[i0] = rkBox.extents[i0]; rkPnt[i1] = fT - rkBox.extents[i1]; rkPnt[i2] = -rkBox.extents[i2]; } } else { fLSqr += rkDir[i1]*rkDir[i1]; fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] + rkDir[i2]*kPpE[i2]; fParam = -fDelta/fLSqr; rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam; if (pf.useIt) { pf.pfLParam = fParam; rkPnt[i0] = rkBox.extents[i0]; rkPnt[i1] = rkBox.extents[i1]; rkPnt[i2] = -rkBox.extents[i2]; } } } } else { if (rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0]) { // v[i1] < -e[i1], v[i2] >= -e[i2] fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]; fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1]); if (fTmp <= 2.0*fLSqr*rkBox.extents[i2]) { fT = fTmp/fLSqr; fLSqr += rkDir[i2]*rkDir[i2]; fTmp = kPpE[i2] - fT; fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*fTmp; fParam = -fDelta/fLSqr; rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + fTmp*fTmp + fDelta*fParam; if (pf.useIt) { pf.pfLParam = fParam; rkPnt[i0] = rkBox.extents[i0]; rkPnt[i1] = -rkBox.extents[i1]; rkPnt[i2] = fT - rkBox.extents[i2]; } } else { fLSqr += rkDir[i2]*rkDir[i2]; fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*rkPmE[i2]; fParam = -fDelta/fLSqr; rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + rkPmE[i2]*rkPmE[i2] + fDelta*fParam; if (pf.useIt) { pf.pfLParam = fParam; rkPnt[i0] = rkBox.extents[i0]; rkPnt[i1] = -rkBox.extents[i1]; rkPnt[i2] = rkBox.extents[i2]; } } } else { // v[i1] < -e[i1], v[i2] < -e[i2] fLSqr = rkDir[i0]*rkDir[i0]+rkDir[i2]*rkDir[i2]; fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] + rkDir[i2]*kPpE[i2]); if (fTmp >= 0.0f) { // v[i1]-edge is closest if (fTmp <= 2.0*fLSqr*rkBox.extents[i1]) { fT = fTmp/fLSqr; fLSqr += rkDir[i1]*rkDir[i1]; fTmp = kPpE[i1] - fT; fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp + rkDir[i2]*kPpE[i2]; fParam = -fDelta/fLSqr; rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp + kPpE[i2]*kPpE[i2] + fDelta*fParam; if (pf.useIt) { pf.pfLParam = fParam; rkPnt[i0] = rkBox.extents[i0]; rkPnt[i1] = fT - rkBox.extents[i1]; rkPnt[i2] = -rkBox.extents[i2]; } } else { fLSqr += rkDir[i1]*rkDir[i1]; fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] + rkDir[i2]*kPpE[i2]; fParam = -fDelta/fLSqr; rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam; if (pf.useIt) { pf.pfLParam = fParam; rkPnt[i0] = rkBox.extents[i0]; rkPnt[i1] = rkBox.extents[i1]; rkPnt[i2] = -rkBox.extents[i2]; } } return; } fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]; fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1]); if (fTmp >= 0.0f) { // v[i2]-edge is closest if (fTmp <= 2.0*fLSqr*rkBox.extents[i2]) { fT = fTmp/fLSqr; fLSqr += rkDir[i2]*rkDir[i2]; fTmp = kPpE[i2] - fT; fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*fTmp; fParam = -fDelta/fLSqr; rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + fTmp*fTmp + fDelta*fParam; if (pf.useIt) { pf.pfLParam = fParam; rkPnt[i0] = rkBox.extents[i0]; rkPnt[i1] = -rkBox.extents[i1]; rkPnt[i2] = fT - rkBox.extents[i2]; } } else { fLSqr += rkDir[i2]*rkDir[i2]; fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*rkPmE[i2]; fParam = -fDelta/fLSqr; rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + rkPmE[i2]*rkPmE[i2] + fDelta*fParam; if (pf.useIt) { pf.pfLParam = fParam; rkPnt[i0] = rkBox.extents[i0]; rkPnt[i1] = -rkBox.extents[i1]; rkPnt[i2] = rkBox.extents[i2]; } } return; } // (v[i1],v[i2])-corner is closest fLSqr += rkDir[i2]*rkDir[i2]; fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*kPpE[i2]; fParam = -fDelta/fLSqr; rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam; if (pf.useIt) { pf.pfLParam = fParam; rkPnt[i0] = rkBox.extents[i0]; rkPnt[i1] = -rkBox.extents[i1]; rkPnt[i2] = -rkBox.extents[i2]; } } } }
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(); }
public static float SqrDistSegOBB(Segment rkSeg, CollisionOBB rkBox, SegOBBParams pf) { Segment kLine = new Segment(rkSeg.origin, rkSeg.direction); float fSqrDistance = SqrDistSegOBBInternal(kLine,rkBox,pf); if (pf.pfLParam >= 0.0f) { if (pf.pfLParam <= 1.0f) { return fSqrDistance; } else { fSqrDistance = XSqDistPtOBB(rkSeg.origin + rkSeg.direction,rkBox,pf); if (pf.useIt) pf.pfLParam = 1.0f; return fSqrDistance; } } else { fSqrDistance = XSqDistPtOBB(rkSeg.origin,rkBox,pf); if (pf.useIt) pf.pfLParam = 0.0f; return fSqrDistance; } }
public static void Case0(int i0, int i1, int i2, ref Vector3 rkPnt, Vector3 rkDir, CollisionOBB rkBox, SegOBBParams pf, ref float rfSqrDistance) { float fPmE0 = rkPnt[i0] - rkBox.extents[i0]; float fPmE1 = rkPnt[i1] - rkBox.extents[i1]; float fProd0 = rkDir[i1]*fPmE0; float fProd1 = rkDir[i0]*fPmE1; float fDelta, fInvLSqr, fInv; if (fProd0 >= fProd1) { // line intersects P[i0] = e[i0] rkPnt[i0] = rkBox.extents[i0]; float fPpE1 = rkPnt[i1] + rkBox.extents[i1]; fDelta = fProd0 - rkDir[i0]*fPpE1; if (fDelta >= 0.0f) { fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]); rfSqrDistance += fDelta*fDelta*fInvLSqr; if (pf.useIt) { rkPnt[i1] = -rkBox.extents[i1]; pf.pfLParam = -(rkDir[i0]*fPmE0+rkDir[i1]*fPpE1)*fInvLSqr; } } else { if (pf.useIt) { fInv = 1.0f/rkDir[i0]; rkPnt[i1] -= fProd0*fInv; pf.pfLParam = -fPmE0*fInv; } } } else { // line intersects P[i1] = e[i1] rkPnt[i1] = rkBox.extents[i1]; float fPpE0 = rkPnt[i0] + rkBox.extents[i0]; fDelta = fProd1 - rkDir[i1]*fPpE0; if (fDelta >= 0.0f) { fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]); rfSqrDistance += fDelta*fDelta*fInvLSqr; if (pf.useIt) { rkPnt[i0] = -rkBox.extents[i0]; pf.pfLParam = -(rkDir[i0]*fPpE0+rkDir[i1]*fPmE1)*fInvLSqr; } } else { if (pf.useIt) { fInv = 1.0f/rkDir[i1]; rkPnt[i0] -= fProd1*fInv; pf.pfLParam = -fPmE1*fInv; } } } if (rkPnt[i2] < -rkBox.extents[i2]) { fDelta = rkPnt[i2] + rkBox.extents[i2]; rfSqrDistance += fDelta*fDelta; rkPnt[i2] = -rkBox.extents[i2]; } else if (rkPnt[i2] > rkBox.extents[i2]) { fDelta = rkPnt[i2] - rkBox.extents[i2]; rfSqrDistance += fDelta*fDelta; rkPnt[i2] = rkBox.extents[i2]; } }
public static bool TestCapsuleAABB(CollisionCapsule a, CollisionAABB b, CollisionParms parms) { // Convert the AABB into an OBB, then run the OBBOBB test. // Not the most efficient algorithm but it gets the job // done. CollisionOBB newB = new CollisionOBB(b.center, UnitBasisVectors, (b.max - b.min) * .5f); return TestCapsuleOBB(a, newB, parms); }
// Returns true if sphere s intersects OBB b, false otherwise. // The point p on the OBB closest to the sphere center is also returned public static bool TestSphereOBB(CollisionSphere s, CollisionOBB b, out Vector3 p) { return (SqDistPointOBB(s.center, b, out p) < s.radius * s.radius); }
public static bool TestCollisionAABBOBB(CollisionShape s1, CollisionShape s2, CollisionParms parms) { CollisionAABB x1 = (CollisionAABB)s1; CollisionOBB x2 = (CollisionOBB)s2; // Convert the AABB into an OBB, then run the OBBOBB test. // Not the most efficient algorithm but it gets the job // done. CollisionOBB newx1 = new CollisionOBB(x1.center, UnitBasisVectors, (x1.max - x1.min) * .5f); return TestOBBOBB(newx1, x2, parms); }
public static float XSqDistPtOBB(Vector3 rkPoint, CollisionOBB rkBox, SegOBBParams pf) { // compute coordinates of point in box coordinate system Vector3 kDiff = rkPoint - rkBox.center; Vector3 kClosest = new Vector3(kDiff.Dot(rkBox.axes[0]), kDiff.Dot(rkBox.axes[1]), kDiff.Dot(rkBox.axes[2])); // project test point onto box float fSqrDistance = 0.0f; float fDelta; for (int i=0; i<3; i++) { if (kClosest[i] < -rkBox.extents[i]) { fDelta = kClosest[i] + rkBox.extents[i]; fSqrDistance += fDelta*fDelta; kClosest[i] = -rkBox.extents[i]; } else if (kClosest[i] > rkBox.extents[i]) { fDelta = kClosest[i] - rkBox.extents[i]; fSqrDistance += fDelta*fDelta; kClosest[i] = rkBox.extents[i]; } } if (pf.useIt) { pf.pfBVec = kClosest; } return fSqrDistance; }
public static bool TestOBBOBBInternal(CollisionOBB a, CollisionOBB b) { float ra, rb; Matrix3 R = Matrix3.Zero; Matrix3 AbsR = Matrix3.Zero; // Compute rotation matrix expressing b in a's coordinate frame for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) R[i,j] = a.axes[i].Dot(b.axes[j]); // Compute translation vector t Vector3 t = b.center - a.center; // Bring translation into a's coordinate frame t = new Vector3(t.Dot(a.axes[0]), t.Dot(a.axes[1]), t.Dot(a.axes[2])); // Compute common subexpressions. Add in an epsilon term to // counteract arithmetic errors when two edges are parallel and // their cross product is (near) null (see text for details) for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) AbsR[i,j] = Math.Abs(R[i,j]) + epsilon; // Test axes L = A0, L = A1, L = A2 for (int i = 0; i < 3; i++) { ra = a.extents[i]; rb = b.extents[0] * AbsR[i,0] + b.extents[1] * AbsR[i,1] + b.extents[2] * AbsR[i,2]; if (Math.Abs(t[i]) > ra + rb) return false; } // Test axes L = B0, L = B1, L = B2 for (int i = 0; i < 3; i++) { ra = a.extents[0] * AbsR[0,i] + a.extents[1] * AbsR[1,i] + a.extents[2] * AbsR[2,i]; rb = b.extents[i]; if (Math.Abs(t[0] * R[0,i] + t[1] * R[1,i] + t[2] * R[2,i]) > ra + rb) return false; } // Test axis L = A0 x B0 ra = a.extents[1] * AbsR[2,0] + a.extents[2] * AbsR[1,0]; rb = b.extents[1] * AbsR[0,2] + b.extents[2] * AbsR[0,1]; if (Math.Abs(t[2] * R[1,0] - t[1] * R[2,0]) > ra + rb) return false; // Test axis L = A0 x B1 ra = a.extents[1] * AbsR[2,1] + a.extents[2] * AbsR[1,1]; rb = b.extents[0] * AbsR[0,2] + b.extents[2] * AbsR[0,0]; if (Math.Abs(t[2] * R[1,1] - t[1] * R[2,1]) > ra + rb) return false; // Test axis L = A0 x B2 ra = a.extents[1] * AbsR[2,2] + a.extents[2] * AbsR[1,2]; rb = b.extents[0] * AbsR[0,1] + b.extents[1] * AbsR[0,0]; if (Math.Abs(t[2] * R[1,2] - t[1] * R[2,2]) > ra + rb) return false; // Test axis L = A1 x B0 ra = a.extents[0] * AbsR[2,0] + a.extents[2] * AbsR[0,0]; rb = b.extents[1] * AbsR[1,2] + b.extents[2] * AbsR[1,1]; if (Math.Abs(t[0] * R[2,0] - t[2] * R[0,0]) > ra + rb) return false; // Test axis L = A1 x B1 ra = a.extents[0] * AbsR[2,1] + a.extents[2] * AbsR[0,1]; rb = b.extents[0] * AbsR[1,2] + b.extents[2] * AbsR[1,0]; if (Math.Abs(t[0] * R[2,1] - t[2] * R[0,1]) > ra + rb) return false; // Test axis L = A1 x B2 ra = a.extents[0] * AbsR[2,2] + a.extents[2] * AbsR[0,2]; rb = b.extents[0] * AbsR[1,1] + b.extents[1] * AbsR[1,0]; if (Math.Abs(t[0] * R[2,2] - t[2] * R[0,2]) > ra + rb) return false; // Test axis L = A2 x B0 ra = a.extents[0] * AbsR[1,0] + a.extents[1] * AbsR[0,0]; rb = b.extents[1] * AbsR[2,2] + b.extents[2] * AbsR[2,1]; if (Math.Abs(t[1] * R[0,0] - t[0] * R[1,0]) > ra + rb) return false; // Test axis L = A2 x B1 ra = a.extents[0] * AbsR[1,1] + a.extents[1] * AbsR[0,1]; rb = b.extents[0] * AbsR[2,2] + b.extents[2] * AbsR[2,0]; if (Math.Abs(t[1] * R[0,1] - t[0] * R[1,1]) > ra + rb) return false; // Test axis L = A2 x B2 ra = a.extents[0] * AbsR[1,2] + a.extents[1] * AbsR[0,2]; rb = b.extents[0] * AbsR[2,1] + b.extents[1] * AbsR[2,0]; if (Math.Abs(t[1] * R[0,2] - t[0] * R[1,2]) > ra + rb) return false; // Since no separating axis found, the OBBs must be intersecting return true; }
public static void Case00(int i0, int i1, int i2, ref Vector3 rkPnt, Vector3 rkDir, CollisionOBB rkBox, SegOBBParams pf, ref float rfSqrDistance) { float fDelta; if (pf.useIt) pf.pfLParam = (rkBox.extents[i0] - rkPnt[i0])/rkDir[i0]; rkPnt[i0] = rkBox.extents[i0]; if (rkPnt[i1] < -rkBox.extents[i1]) { fDelta = rkPnt[i1] + rkBox.extents[i1]; rfSqrDistance += fDelta*fDelta; rkPnt[i1] = -rkBox.extents[i1]; } else if (rkPnt[i1] > rkBox.extents[i1]) { fDelta = rkPnt[i1] - rkBox.extents[i1]; rfSqrDistance += fDelta*fDelta; rkPnt[i1] = rkBox.extents[i1]; } if (rkPnt[i2] < -rkBox.extents[i2]) { fDelta = rkPnt[i2] + rkBox.extents[i2]; rfSqrDistance += fDelta*fDelta; rkPnt[i2] = -rkBox.extents[i2]; } else if (rkPnt[i2] > rkBox.extents[i2]) { fDelta = rkPnt[i2] - rkBox.extents[i2]; rfSqrDistance += fDelta*fDelta; rkPnt[i2] = rkBox.extents[i2]; } }