public override CollisionShape Clone() { CollisionAABB rv = new CollisionAABB(min, max); rv.handle = this.handle; rv.timeStamp = this.timeStamp; return(rv); }
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 PathGenerator(bool logPathGeneration, string modelName, PathObjectType poType, float terrainLevel, Matrix4 modelTransform, List<CollisionShape> collisionVolumes) { this.dumpGrid = logPathGeneration; this.modelName = modelName; this.poType = poType; this.modelWidth = poType.width * oneMeter; this.cellWidth = poType.gridResolution * modelWidth; this.boxHeight = poType.height * oneMeter; this.maxClimbDistance = poType.maxClimbSlope * cellWidth; this.maxDisjointDistance = poType.maxDisjointDistance * oneMeter; this.minimumFeatureSize = poType.minimumFeatureSize; this.terrainLevel = terrainLevel; this.modelTransform = modelTransform; this.collisionVolumes = collisionVolumes; stopwatch = new Stopwatch(); stopwatch.Start(); // Create the collisionAPI object collisionAPI = new CollisionAPI(false); // Ugly workaround for a modularity problem do to // unadvised use of static variables: remember the // existing state of rendering collision volumes RenderedNode.RenderState oldRenderState = RenderedNode.renderState; RenderedNode.renderState = RenderedNode.RenderState.None; // Form the union of the collision volumes; we don't care // about Y coordinate, only the X and Z coordinates Vector3 min = Vector3.Zero; Vector3 max = Vector3.Zero; for (int i=0; i<collisionVolumes.Count; i++) { CollisionShape shape = collisionVolumes[i]; // Add the shape to the sphere tree collisionAPI.SphereTree.AddCollisionShape(shape); AxisAlignedBox vol = shape.BoundingBox(); // If this is the first iteration, set up the min and // max if (i == 0) { min = vol.Minimum; max = vol.Maximum; min.y = terrainLevel; max.y = terrainLevel; continue; } // Enlarge the box by the dimensions of the shape min.x = Math.Min(min.x, vol.Minimum.x); min.z = Math.Min(min.z, vol.Minimum.z); max.x = Math.Max(max.x, vol.Maximum.x); max.z = Math.Max(max.z, vol.Maximum.z); } // Restore RenderState RenderedNode.renderState = oldRenderState; // Round out the max and min by 4 times the grid // resolution, so we can just divide to find the // horizontal and vertical numbers are cells Vector3 margin = Vector3.UnitScale * 4 * cellWidth; min -= margin; max += margin; // Now adjust the max the min coords so that they are // exactly a multiple of the grid resolution min.x = MultipleOfResolution(min.x); min.z = MultipleOfResolution(min.z); max.x = MultipleOfResolution(max.x); max.z = MultipleOfResolution(max.z); // Set the lower left and upper right corners lowerLeftCorner = min; upperRightCorner = max; // Set the horizontal and vertical counts xCount = (int)((max.x - min.x) / cellWidth); zCount = (int)((max.z - min.z) / cellWidth); // Initial the gridBox gridBox = new AxisAlignedBox(min, max); // Allocate the grid grid = new List<GridCell>[xCount, zCount]; for (int i=0; i<xCount; i++) for (int j=0; j<zCount; j++) grid[i,j] = new List<GridCell>(); // Initialize the work list, adding the cell at 0,0 and // the terrain height as the first member workList = new List<CellLocation>(); workList.Add(new CellLocation(0, 0, terrainLevel, null)); // Create the moving box at the center of the 0, 0 cell, // and the MovingObject object that contains the moving box movingBox = new CollisionAABB( new Vector3(min.x, terrainLevel, min.z), new Vector3(min.x + cellWidth, terrainLevel + boxHeight, min.z + terrainLevel)); movingObject = new MovingObject(collisionAPI); movingObject.AddPart(movingBox); }
// This method is the callback by which the forests controlled // by the scene manager tell us about SpeedTrees. private void AddTreeObstacles(SpeedTreeWrapper tree) { // If this tree didn't use to be in range but now is V3 vp = tree.TreePosition; Vector3 p = new Vector3(vp.x, vp.y, vp.z); // Find the tile in question int tileX, tileZ; WorldToTileCoords(p, out tileX, out tileZ); bool oir = InRange(tileXCenter, tileZCenter, tileX, tileZ); bool nir = InRange(newTileXCenter, newTileZCenter, tileX, tileZ); // if (MO.DoLog) // MO.Log(String.Format("For tree at {0}, testing !InRange({1},{2},{3},{4}) = {5} && InRange({6},{7},{8},{9}) = {10}", // MO.MeterString(p),tileXCenter, tileZCenter, tileX, tileZ, MO.Bool(!oir), // newTileXCenter, newTileZCenter, tileX, tileZ, MO.Bool(nir))); if (!oir && nir) { int tX = TileToArrayIndex(tileX, newTileXCenter); int tZ = TileToArrayIndex(tileZ, newTileZCenter); uint count = tree.CollisionObjectCount; long handle = ComposeHandle(tileX, tileZ); CollisionShape shape = null; if (MO.DoLog) { MO.Log(string.Format("Adding tree at {0}, tiles[{1},{2}] = {3}, tile coords[{4},{5}], obj. new count {6}", MO.MeterString(p), tX, tZ, tiles[tX,tZ], tileX, tileZ, count)); MO.Log(string.Format("Handle {0}, oldcenter {1}, newcenter {2}", MO.HandleString(handle), MO.MeterString(tileWorldCenter), MO.MeterString(newTileWorldCenter))); } float size = 0f; float variance = 0f; tree.GetTreeSize(ref size, ref variance); float scaleFactor = size / tree.OriginalSize; for (uint i=0; i<count; i++) { TreeCollisionObject tco = tree.CollisionObject(i); Vector3 cp = new Vector3(tco.position.x, tco.position.y, tco.position.z) * scaleFactor + p; Vector3 cd = new Vector3(tco.dimensions.x, tco.dimensions.y, tco.dimensions.z) * scaleFactor; switch ((CollisionObjectType)tco.type) { case CollisionObjectType.ColSphere: shape = new CollisionSphere(cp, cd.x); break; case CollisionObjectType.ColCylinder: // We treat it as a capsule, but we raise the // capsule up by the capRadius, and shorten // the segment by the double the capRadius Vector3 top = cp; top.y += cd.y - cd.x * 2f; cp.y += cd.x; shape = new CollisionCapsule(cp, top, cd.x); break; case CollisionObjectType.ColBox: Vector3 tp = cp; tp.x -= cd.x * .5f; tp.y -= cd.y * .5f; shape = new CollisionAABB(tp, tp + cd); break; } collisionAPI.AddCollisionShape(shape, handle); tiles[tX, tZ]++; objectsAdded++; if (MO.DoLog) { MO.Log(string.Format(" tiles[{0},{1}] = {2}, tile at [{3},{4}] after adding shape {5}", tX, tZ, tiles[tX, tZ], tileX, tileZ, shape.ToString())); } } } }
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); }
public static bool TestAABBAABB(CollisionAABB a, CollisionAABB b) { // Exit with no intersection if separated along an axis if (a.max[0] < b.min[0] || a.min[0] > b.max[0]) return false; if (a.max[1] < b.min[1] || a.min[1] > b.max[1]) return false; if (a.max[2] < b.min[2] || a.min[2] > b.max[2]) return false; // Overlapping on all axes means AABBs are intersecting return true; }
// Computes the square distance between a point p and an AABB b public static float SqDistPointAABB(Vector3 p, CollisionAABB b) { float sqDist = 0.0f; for (int i = 0; i < 3; i++) { // For each axis count any excess distance outside box extents float v = p[i]; if (v < b.min[i]) sqDist += (b.min[i] - v) * (b.min[i] - v); if (v > b.max[i]) sqDist += (v - b.max[i]) * (v - b.max[i]); } return sqDist; }
// Given point p, return the point q on or in AABB b, that is closest to p public static void ClosestPtPointAABB(Vector3 p, CollisionAABB b, out Vector3 q) { // For each coordinate axis, if the point coordinate value is // outside box, clamp it to the box, else keep it as is q = Vector3.Zero; for (int i = 0; i < 3; i++) { float v = p[i]; v = Clamp(v, b.min[i], b.max[i]); q[i] = v; } }
// Test if segment specified by points p0 and p1 intersects AABB b public static bool TestSegmentAABB(Vector3 p0, Vector3 p1, CollisionAABB b) { Vector3 e = b.max - b.center; // Box halflength extents Vector3 m = (p0 + p1) * 0.5f; // Segment midpoint Vector3 d = p1 - m; // Segment halflength vector m = m - b.center; // Translate box and segment to origin // Try world coordinate axes as separating axes float adx = Math.Abs(d.x); if (Math.Abs(m.x) > e.x + adx) return false; float ady = Math.Abs(d.y); if (Math.Abs(m.y) > e.y + ady) return false; float adz = Math.Abs(d.z); if (Math.Abs(m.z) > e.z + adz) return false; // Add in an epsilon term to counteract arithmetic errors when segment is // (near) parallel to a coordinate axis (see text for detail) adx += epsilon; ady += epsilon; adz += epsilon; // Try cross products of segment direction vector with coordinate axes if (Math.Abs(m.y * d.z - m.z * d.y) > e.y * adz + e.z * ady) return false; if (Math.Abs(m.z * d.x - m.x * d.z) > e.x * adz + e.z * adx) return false; if (Math.Abs(m.x * d.y - m.y * d.x) > e.x * ady + e.y * adx) return false; // No separating axis found; segment must be overlapping AABB return true; }
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 ); } }
// This method is the callback by which the forests controlled // by the scene manager tell us about SpeedTrees. private void AddTreeObstacles(SpeedTreeWrapper tree) { // If this tree didn't use to be in range but now is V3 vp = tree.TreePosition; Vector3 p = new Vector3(vp.x, vp.y, vp.z); // Find the tile in question int tileX, tileZ; WorldToTileCoords(p, out tileX, out tileZ); bool oir = InRange(tileXCenter, tileZCenter, tileX, tileZ); bool nir = InRange(newTileXCenter, newTileZCenter, tileX, tileZ); // if (MO.DoLog) // MO.Log(String.Format("For tree at {0}, testing !InRange({1},{2},{3},{4}) = {5} && InRange({6},{7},{8},{9}) = {10}", // MO.MeterString(p),tileXCenter, tileZCenter, tileX, tileZ, MO.Bool(!oir), // newTileXCenter, newTileZCenter, tileX, tileZ, MO.Bool(nir))); if (!oir && nir) { int tX = TileToArrayIndex(tileX, newTileXCenter); int tZ = TileToArrayIndex(tileZ, newTileZCenter); uint count = tree.CollisionObjectCount; long handle = ComposeHandle(tileX, tileZ); CollisionShape shape = null; if (MO.DoLog) { MO.Log(string.Format("Adding tree at {0}, tiles[{1},{2}] = {3}, tile coords[{4},{5}], obj. new count {6}", MO.MeterString(p), tX, tZ, tiles[tX, tZ], tileX, tileZ, count)); MO.Log(string.Format("Handle {0}, oldcenter {1}, newcenter {2}", MO.HandleString(handle), MO.MeterString(tileWorldCenter), MO.MeterString(newTileWorldCenter))); } float size = 0f; float variance = 0f; tree.GetTreeSize(ref size, ref variance); float scaleFactor = size / tree.OriginalSize; for (uint i = 0; i < count; i++) { TreeCollisionObject tco = tree.CollisionObject(i); Vector3 cp = new Vector3(tco.position.x, tco.position.y, tco.position.z) * scaleFactor + p; Vector3 cd = new Vector3(tco.dimensions.x, tco.dimensions.y, tco.dimensions.z) * scaleFactor; switch ((CollisionObjectType)tco.type) { case CollisionObjectType.ColSphere: shape = new CollisionSphere(cp, cd.x); break; case CollisionObjectType.ColCylinder: // We treat it as a capsule, but we raise the // capsule up by the capRadius, and shorten // the segment by the double the capRadius Vector3 top = cp; top.y += cd.y - cd.x * 2f; cp.y += cd.x; shape = new CollisionCapsule(cp, top, cd.x); break; case CollisionObjectType.ColBox: Vector3 tp = cp; tp.x -= cd.x * .5f; tp.y -= cd.y * .5f; shape = new CollisionAABB(tp, tp + cd); break; } collisionAPI.AddCollisionShape(shape, handle); tiles[tX, tZ]++; objectsAdded++; if (MO.DoLog) { MO.Log(string.Format(" tiles[{0},{1}] = {2}, tile at [{3},{4}] after adding shape {5}", tX, tZ, tiles[tX, tZ], tileX, tileZ, shape.ToString())); } } } }
public override CollisionShape Clone() { CollisionAABB rv = new CollisionAABB(min, max); rv.handle = this.handle; rv.timeStamp = this.timeStamp; return rv; }
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; }