// Test all the parts of a moving object to see if they collide with // any obstacle public bool CollideAfterAddingDisplacement(MovingObject mo, Vector3 step, CollisionParms parms) { for (int i = 0; i < mo.parts.Count; i++) { MovingPart movingPart = mo.parts[i]; collisionTimeStamp++; movingPart.AddDisplacement(step); SphereTreeNode s = sphereTree.FindSmallestContainer(movingPart.shape, movingPart.sphere); //MaybeDumpSphereTree(); movingPart.sphere = s; partCalls++; if (s.TestSphereCollision(movingPart.shape, collisionTimeStamp, ref collisionTestCount, parms)) { // We hit something, so back out the displacements // added to the parts tested so far for (int j = 0; j <= i; j++) { MovingPart displacedPart = mo.parts[j]; displacedPart.AddDisplacement(-step); //MaybeDumpSphereTree(); } return(true); } } return(false); }
public override void SetTerrain(float[] heightMap) { for (int i = 0; i < 65536; i++) { this._heightmap[i] = (double)heightMap[i]; } IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); d.GeomHeightfieldDataBuildDouble(HeightmapData, _heightmap, 0, 256, 256, 256, 256, 1.0f, 0.0f, 2.0f, 0); d.GeomHeightfieldDataSetBounds(HeightmapData, 256, 256); LandGeom = d.CreateHeightfield(space, HeightmapData, 1); d.Matrix3 R = new d.Matrix3(); Axiom.MathLib.Quaternion q1 = Axiom.MathLib.Quaternion.FromAngleAxis(1.5707f, new Axiom.MathLib.Vector3(1, 0, 0)); Axiom.MathLib.Quaternion q2 = Axiom.MathLib.Quaternion.FromAngleAxis(1.5707f, new Axiom.MathLib.Vector3(0, 1, 0)); //Axiom.MathLib.Quaternion q3 = Axiom.MathLib.Quaternion.FromAngleAxis(3.14f, new Axiom.MathLib.Vector3(0, 0, 1)); q1 = q1 * q2; //q1 = q1 * q3; Axiom.MathLib.Vector3 v3 = new Axiom.MathLib.Vector3(); float angle = 0; q1.ToAngleAxis(ref angle, ref v3); d.RFromAxisAndAngle(out R, v3.x, v3.y, v3.z, angle); d.GeomSetRotation(LandGeom, ref R); d.GeomSetPosition(LandGeom, 128, 128, 0); }
public void AddDisplacement(Vector3 displacement) { foreach (MovingPart part in parts) { part.AddDisplacement(displacement); } }
public bool TestCollision(MovingObject mo, ref Vector3 displacement, CollisionParms parms) { // Find the minimum step size for the assembly of parts float stepSize = mo.StepSize(displacement); return(TestCollision(mo, stepSize, ref displacement, parms)); }
public void Transform(Vector3 scale, Quaternion rotate, Vector3 translate) { foreach (MovingPart part in parts) { part.Transform(scale, rotate, translate); } }
private bool StepTowardCollision(MovingObject mo, Vector3 displacement, int nsteps, Vector3 step, CollisionParms parms) { Vector3 accumulatedSteps = Vector3.Zero; for (int i = 0; i < nsteps; i++) { Vector3 nextStep; if (i == nsteps - 1) { nextStep = displacement - accumulatedSteps; } else { nextStep = step; } if (CollideAfterAddingDisplacement(mo, nextStep, parms)) { topLevelCollisions++; return(true); } accumulatedSteps += nextStep; } // No collision! parms.obstacle = null; return(false); }
public CollisionOBB(Vector3 center, Vector3[] axes, Vector3 extents) { this.center = center; this.axes = axes; this.extents = extents; this.radius = extents.Length; }
public override bool PointInside(Vector3 p) { Vector3 junk; return(PointInsideSphere(p) && Primitives.SqDistPointOBB(p, this, out junk) < ScaleEpsilon); }
public void Initialize() { part = null; obstacle = null; swapped = false; normPart = Vector3.Zero; normObstacle = Vector3.Zero; }
private static RenderedNode NewRenderedObject(string meshName, int id, int index, Vector3 position, float scale) { RenderedNode n = UnscaledRenderedObject(meshName, id, index, position); n.node.ScaleFactor = Vector3.UnitScale * scale; return(n); }
private static RenderedNode NewRenderedObject(string meshName, int id, int index, Vector3 position, Vector3 scale, Quaternion orientation) { RenderedNode n = UnscaledRenderedObject(meshName, id, index, position); n.node.ScaleFactor = scale; n.node.Orientation = orientation; return(n); }
protected static bool IsUniformScale(Vector3 scale) { if (Math.Abs(scale.x - scale.y) > ScaleEpsilon || Math.Abs(scale.y - scale.z) > ScaleEpsilon) { return(false); } return(true); }
public CollisionCapsule(Vector3 bottomcenter, Vector3 topcenter, float capRadius) { this.bottomcenter = bottomcenter; this.topcenter = topcenter; this.capRadius = capRadius; this.height = (topcenter - bottomcenter).Length; this.center = (topcenter + bottomcenter) / 2; this.radius = height + capRadius; }
public float StepSize(Vector3 displacement) { float step = float.MaxValue; foreach (MovingPart part in parts) { step = Math.Min(step, part.shape.StepSize(displacement)); } return(step); }
public override float StepSize(Vector3 displacement) { float s = float.MaxValue; for (int i = 0; i < 3; i++) { s = Math.Min(s, extents[i]); } return(s * 2 * PunchThroughFraction); }
public static void AddDisplacement(Vector3 displacement, ref List <RenderedNode> renderedNodes) { if (renderedNodes != null) { for (int i = 0; i < renderedNodes.Count; i++) { RenderedNode n = renderedNodes[i]; n.node.Position += displacement; } } }
public void SetNormObstacle(Vector3 norm) { if (swapped) { normPart = norm; } else { normObstacle = norm; } }
public bool PointInside(Vector3 p) { foreach (CollisionShape shape in shapes) { if (shape.PointInside(p)) { return(true); } } return(false); }
public override Vector3 Corner(int cornerNumber) { Vector3 cornerSum = center; for (int i = 0; i < 3; i++) { float multiplier = ((1 << i) & cornerNumber) == 0 ? 1f : -1f; cornerSum += multiplier * extents[i] * axes[i]; } return(cornerSum); }
public override float StepSize(Vector3 displacement) { float s = float.MaxValue; for (int i = 0; i < 3; i++) { float d = max[i] - min[i]; s = Math.Min(s, d); } return(s * PunchThroughFraction); }
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); }
private int RayIntersectionDistanceInternal(Vector3 start, Vector3 end, out Vector3 [] afT) { int riQuantity; // convert ray to box coordinates Vector3 direction = end - start; Vector3 kDiff = start - center; Vector3 kOrigin = new Vector3(kDiff.Dot(axes[0]), kDiff.Dot(axes[1]), kDiff.Dot(axes[2])); Vector3 kDirection = new Vector3(direction.Dot(axes[0]), direction.Dot(axes[1]), direction.Dot(axes[2])); float fT0 = 0.0f; float fT1 = float.MaxValue; afT = new Vector3[] { Vector3.Zero, Vector3.Zero }; bool bIntersects = FindIntersection(kOrigin, kDirection, ref fT0, ref fT1); if (bIntersects) { if (fT0 > 0.0f) { if (fT1 <= 1.0f) { riQuantity = 2; afT[0] = start + fT0 * direction; afT[1] = start + fT1 * direction; } else { riQuantity = 1; afT[0] = start + fT0 * direction; } } else // fT0 == 0.0 { if (fT1 <= 1.0f) { riQuantity = 1; afT[0] = start + fT1 * direction; } else // fT1 == INFINITY // assert: should not get here { riQuantity = 0; } } } else { riQuantity = 0; } return(riQuantity); }
public void Transform(Vector3 scale, Quaternion rotate, Vector3 translate) { // TODO: This is a temporary solution, and terribly inefficient. // I need to update the transforms on the node properly, without // the remove/add. RenderedNode.RemoveNodeRendering(id, ref renderedNodes); shape = constantShape.Clone(); shape.Transform(scale, rotate, translate); id = api.SphereTree.GetId(); renderedNodes = null; RenderedNode.MaybeCreateRenderedNodes(false, shape, id, ref renderedNodes); sphere = null; }
public override void Transform(Vector3 scale, Quaternion rotate, Vector3 translate) { if (!IsUniformScale(scale)) { throw new Exception("Unsupported non-uniform scale on Sphere"); } // Update radius and center to take the scale into account radius *= scale.x; center *= scale.x; // We can ignore the rotate for a sphere // Update the center to take the translate into account center += translate; }
public CollisionAABB(Vector3 min, Vector3 max) { float s = 0.0f; this.min = min; this.max = max; for (int i = 0; i < 3; i++) { this.center[i] = (min[i] + max[i]) / 2; s += ((max[i] - min[i]) * (max[i] - min[i])); } this.radius = (float)Math.Sqrt((double)(s / 4)); }
// This constructor for the top node public SphereTreeNode(Vector3 center, float radius, SphereTree sphereTree) : base(center, radius) { this.sphereTree = sphereTree; parent = null; children = new SphereTreeNode[childCount]; containedShape = null; intersectingShapes = new List <CollisionShape>(); leafNode = false; sphereTree.idCounter++; id = sphereTree.idCounter; sphereTree.nodeCount++; }
public static string AxisString(Vector3 v) { string s = "("; for (int i=0; i<3; i++) { float f = v[i]; if (f == 0f || f == 1f || f == -1f) s += (int)f; else s += string.Format("{0:#.000}", f); if (i < 2) s += ","; } return s + ")"; }
bool FindIntersection(Vector3 start, Vector3 direction, ref float rfT0, ref float rfT1) { float fSaveT0 = rfT0; float fSaveT1 = rfT1; bool bNotEntirelyClipped = Clip(+direction.x, -start.x - extents[0], ref rfT0, ref rfT1) && Clip(-direction.x, +start.x - extents[0], ref rfT0, ref rfT1) && Clip(+direction.y, -start.y - extents[1], ref rfT0, ref rfT1) && Clip(-direction.y, +start.y - extents[1], ref rfT0, ref rfT1) && Clip(+direction.z, -start.z - extents[2], ref rfT0, ref rfT1) && Clip(-direction.z, +start.z - extents[2], ref rfT0, ref rfT1); return(bNotEntirelyClipped && (rfT0 != fSaveT0 || rfT1 != fSaveT1)); }
public override float RayIntersectionDistance(Vector3 start, Vector3 end) { // set up quadratic Q(t) = a*t^2 + 2*b*t + c Vector3 kDiff = start - center; Vector3 direction = end - start; float fA = direction.LengthSquared; float fB = kDiff.Dot(direction); float fC = kDiff.LengthSquared - radius * radius; float afT0, afT1; float fDiscr = fB * fB - fA * fC; if (fDiscr < 0.0) { return(float.MaxValue); } else if (fDiscr > 0.0) { float fRoot = (float)Math.Sqrt(fDiscr); float fInvA = 1.0f / fA; afT0 = (-fB - fRoot) * fInvA; afT1 = (-fB + fRoot) * fInvA; if (afT1 >= 0.0) { return(Math.Min((afT0 * direction).Length, (afT1 * direction).Length)); } else if (afT1 >= 0.0) { return(((start + afT1 * direction) - start).Length); } else { return(float.MaxValue); } } else { afT0 = -fB / fA; if (afT0 >= 0.0) { return(afT0 * direction.Length); } else { return(float.MaxValue); } } }
public bool PointInCollisionVolume(Vector3 p, long handle) { List <CollisionShape> shapes = sphereTree.GetCollisionShapesWithHandle(handle); if (shapes.Count > 0) { foreach (CollisionShape shape in shapes) { if (shape.PointInside(p)) { return(true); } } } return(false); }
public override AxisAlignedBox BoundingBox() { Vector3 min = center; Vector3 max = center; for (int i = 0; i < 8; i++) { Vector3 point = Corner(i); min.x = Math.Min(min.x, point.x); min.y = Math.Min(min.y, point.y); min.z = Math.Min(min.z, point.z); max.x = Math.Max(max.x, point.x); max.y = Math.Max(max.y, point.y); max.z = Math.Max(max.z, point.z); } return(new AxisAlignedBox(min, max)); }
public void AddDisplacement(Vector3 displacement) { shape.AddDisplacement(displacement); RenderedNode.AddDisplacement(displacement, ref renderedNodes); }
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 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; }
// Distance from a segment defined by a and b to a point c public static float SqDistPointSegment(Vector3 a, Vector3 b, Vector3 c) { Vector3 ab = b - a; Vector3 ac = c - a; Vector3 bc = c - b; float e = ac.Dot(ab); // Handle cases where c projects outside ab if (e <= 0.0f) return ac.Dot(ac); float f = ab.Dot(ab); if (e >= f) return bc.Dot(bc); // Handle case where c projects onto ab return ac.Dot(ac) - e * e / f; }
internal GridPolygon(CellStatus status, GridCell[] corners, Vector3[] cornerLocs) { this.status = status; this.corners = corners; this.cornerLocs = cornerLocs; }
public void AddDisplacement(Vector3 displacement) { foreach(MovingPart part in parts) part.AddDisplacement(displacement); }
public static float DistanceSquared(Vector3 v1, Vector3 v2) { Vector3 diff = v2 - v1; return diff.Dot(diff); }
public static float Distance(Vector3 v1, Vector3 v2) { Vector3 diff = v2 - v1; return (float)Math.Sqrt(diff.Dot(diff)); }
// Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and // S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared // distance between between S1(s) and S2(t) public static float ClosestPtSegmentSegment(Vector3 p1, Vector3 q1, Vector3 p2, Vector3 q2, out float s, out float t, out Vector3 c1, out Vector3 c2) { Vector3 d1 = q1 - p1; // Direction vector of segment S1 Vector3 d2 = q2 - p2; // Direction vector of segment S2 Vector3 r = p1 - p2; float a = d1.Dot(d1); // Squared length of segment S1, always nonnegative float e = d2.Dot(d2); // Squared length of segment S2, always nonnegative float f = d2.Dot(r); // Check if either or both segments degenerate into points if (a <= epsilon && e <= epsilon) { // Both segments degenerate into points s = t = 0.0f; c1 = p1; c2 = p2; return DistanceSquared(c1, c2); } if (a <= epsilon) { // First segment degenerates into a point s = 0.0f; t = f / e; // s = 0 => t = (b*s + f) / e = f / e t = Clamp01(t); } else { float c = d1.Dot(r); if (e <= epsilon) { // Second segment degenerates into a point t = 0.0f; s = Clamp01(-c / a); // t = 0 => s = (b*t - c) / a = -c / a } else { // The general nondegenerate case starts here float b = d1.Dot(d2); float denom = a*e-b*b; // Always nonnegative // If segments not parallel, compute closest point on L1 to L2, and // clamp to segment S1. Else pick arbitrary s (here 0) if (denom != 0.0f) { s = Clamp01((b*f - c*e) / denom); } else s = 0.0f; // Compute point on L2 closest to S1(s) using // t = D2.Dot((P1+D1*s)-P2) / D2.Dot(D2) = (b*s + f) / e t = (b*s + f) / e; // If t in [0,1] done. Else clamp t, recompute s for the new value // of t using s = D1.Dot((P2+D2*t)-P1) / D1.Dot(D1)= (t*b - c) / a // and clamp s to [0, 1] if (t < 0.0f) { t = 0.0f; s = Clamp01(-c / a); } else if (t > 1.0f) { t = 1.0f; s = Clamp01((b - c) / a); } } } c1 = p1 + d1 * s; c2 = p2 + d2 * t; return DistanceSquared(c1, c2); }
// Given segment ab and point c, computes closest point d on ab. // Also returns t for the position of d, d(t) = a + t*(b - a) public static void ClosestPtPointSegment(Vector3 c, Vector3 a, Vector3 b, out float t, out Vector3 d) { Vector3 ab = b - a; // Project c onto ab, computing parameterized position d(t) = a + t*(b - a) Vector3 cma = c - a; t = cma.Dot(ab) / ab.Dot(ab); // If outside segment, clamp t (and therefore d) to the closest endpoint t = Clamp01(t); // Compute projected position from the clamped t d = a + t * ab; }
// 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]; } }
// 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; } }
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]; } } } }
public float StepSize(Vector3 displacement) { float step = float.MaxValue; foreach (MovingPart part in parts) step = Math.Min(step, part.shape.StepSize(displacement)); return step; }
// 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; }
public override void SetTerrain(float[] heightMap) { for (int i = 0; i < 65536; i++) { this._heightmap[i] = (double)heightMap[i]; } IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); d.GeomHeightfieldDataBuildDouble(HeightmapData, _heightmap, 0, 256, 256, 256, 256, 1.0f, 0.0f, 2.0f, 0); d.GeomHeightfieldDataSetBounds(HeightmapData, 256, 256); LandGeom = d.CreateHeightfield(space, HeightmapData, 1); d.Matrix3 R = new d.Matrix3(); Axiom.MathLib.Quaternion q1 =Axiom.MathLib.Quaternion.FromAngleAxis(1.5707f, new Axiom.MathLib.Vector3(1,0,0)); Axiom.MathLib.Quaternion q2 =Axiom.MathLib.Quaternion.FromAngleAxis(1.5707f, new Axiom.MathLib.Vector3(0,1,0)); //Axiom.MathLib.Quaternion q3 = Axiom.MathLib.Quaternion.FromAngleAxis(3.14f, new Axiom.MathLib.Vector3(0, 0, 1)); q1 = q1 * q2; //q1 = q1 * q3; Axiom.MathLib.Vector3 v3 = new Axiom.MathLib.Vector3(); float angle = 0; q1.ToAngleAxis(ref angle, ref v3); d.RFromAxisAndAngle(out R, v3.x, v3.y, v3.z, angle); d.GeomSetRotation(LandGeom, ref R); d.GeomSetPosition(LandGeom, 128, 128, 0); }
// 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); }
public void AddDisplacementToRenderedNodes(Vector3 displacement) { RenderedNode.AddDisplacement(displacement, ref renderedNodes); }
///<summary> /// Run the grid traversal algorithm, pushing the cells /// around the model ///</summary> protected void TraverseGridCells() { cellsProcessed = 0; cellsOnCVs = 0; Stopwatch collisionStopwatch = new Stopwatch(); int collisionCount = 0; // Create the CollisionParms object CollisionParms parms = new CollisionParms(); // Set the big step size to 5% of the box height; this // will make the small step size .5% of the box height. // For a box height of 1.8 meters, this is .009m, or 9mm // Since the grid resolution is .25 of model width, and // for the human model, that width is .5m, the cell width // is .125m or 125mm. So .009 / .125 = .072, so the // maximum variation in slope due exclusively to the step // size is 7.2% float stepSize = boxHeight * .05f; // Iterate over work list items until there aren't any while (workList.Count > 0) { CellLocation loc = workList[0]; workList.RemoveAt(0); GridCell cell = FindCellAtHeight(loc); if (cell != null) // Skip, because it's already been visited continue; // Position the moving object over the cell SetMovingBoxOverCell(loc); // If we're above terrain level, we need to drop the // cell. If we're at or below terrain level, we just // mark the cell as supported by the terrain float distanceToTerrain = movingBox.min.y - terrainLevel; if (distanceToTerrain <= 0f) { loc.height = terrainLevel; if (FindCellAtHeight(loc) != null) continue; cell = new GridCell(loc); cell.status = CellStatus.OverTerrain; } else { // Now drop it until it hits a collision object, or // it's at terrain level. Note: this means that we // can't have "basements" until we find a way to have // a non-constant terrain level Vector3 displacement = new Vector3(0, -distanceToTerrain, 0); collisionCount++; collisionStopwatch.Start(); bool hit = collisionAPI.TestCollision(movingObject, stepSize, ref displacement, parms); collisionStopwatch.Stop(); float oldHeight = loc.height; loc.height = movingBox.min.y; if (FindCellAtHeight(loc) != null) continue; cell = new GridCell(loc); if (hit) { // We hit a collision object - - if it's below // us, then set the height accordingly. If // it's not below us, mark the cell as inaccessible. if (displacement.y != -distanceToTerrain) { cell.status = CellStatus.OverCV; cell.supportingShape = parms.obstacle; cellsOnCVs++; } else { loc.height = oldHeight; if (FindCellAtHeight(loc) != null) continue; cell.loc.height = oldHeight; cell.status = CellStatus.Inaccessible; } } else { loc.height = terrainLevel; cell.loc.height = terrainLevel; cell.status = CellStatus.OverTerrain; if (FindCellAtHeight(loc) != null) continue; } } // Add the cell to the grid, now that we know its // actual height cellsProcessed++; grid[loc.xCell, loc.zCell].Add(cell); if (cell.status == CellStatus.Inaccessible) continue; // Now add the neighbors to the work list, if they // haven't already been visited for (GridDirection dir = GridDirection.PlusX; dir <= GridDirection.MinusZ; dir++) { int neighborX = loc.xCell + XIncrement[(int)dir]; int neighborZ = loc.zCell + ZIncrement[(int)dir]; // If the neighbor is outside the grid, ignore it if (neighborX < 0 || neighborX >= xCount || neighborZ < 0 || neighborZ >= zCount) continue; // Test to see if it exists; if so, it's been visited CellLocation neighborLoc = new CellLocation(neighborX, neighborZ, cell.loc.height, loc); GridCell neighborCell = FindCellAtHeight(neighborLoc); // If it doesn't exist, add it to the work queue if (neighborCell == null) workList.Add(neighborLoc); //AddToWorkList(neighborLoc); } } DumpCurrentTime(string.Format("Processing {0} box drops, {1} collision tests, took {2} ms", collisionCount, collisionAPI.partCalls, collisionStopwatch.ElapsedMilliseconds)); DumpGrid("Prefiltered Grid", GridDumpKind.Cells, false); }
public void Transform(Vector3 scale, Quaternion rotate, Vector3 translate) { foreach(MovingPart part in parts) part.Transform(scale, rotate, translate); }
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 SegOBBParams(bool useIt) { this.useIt = useIt; pfLParam = 0.0f; Vector3 pfBVec = new Vector3(0.0f, 0.0f, 0.0f); }
private void generateButton_Click(object sender, EventArgs e) { Cursor previousCursor = Cursor.Current; Cursor.Current = Cursors.WaitCursor; doneLabel.Visible = false; //outputPictureBox.Visible = false; if (generateLogFile.Checked) { string p = "NormalBump.log"; FileStream f = new FileStream(p, FileMode.Create, FileAccess.Write); logStream = new StreamWriter(f); logStream.Write(string.Format("{0} Started writing to {1}\n", DateTime.Now.ToString("hh:mm:ss"), p)); } // run the algorithm Bitmap normalMap = new Bitmap(normalMapTextBox.Text); Bitmap bumpMap = new Bitmap(bumpMapTextBox.Text); if (normalMap.Width != bumpMap.Width) { ShowError("Normal Map width {0} is not the same as Bump Map width {1}", normalMap.Width, bumpMap.Width); return; } if (normalMap.Height != bumpMap.Height) { ShowError("Normal Map height {0} is not the same as Bump Map height {1}", normalMap.Height, bumpMap.Height); return; } Bitmap outputMap = (Bitmap)normalMap.Clone(); PixelFormat normalFormat = normalMap.PixelFormat; PixelFormat bumpFormat = bumpMap.PixelFormat; // This will be set by the slider float scaleFactor = (float)trackBar.Value / 100f; if (reverseBumpDirection.Checked) scaleFactor = - scaleFactor; Vector3 unitZ = new Vector3(0f, 0f, 1f); float epsilon = 0.0000001f; // Loop through the bump map pixels, computing the normals // into the output map int w = normalMap.Width; int h = normalMap.Height; for(int x=0; x < w; x++) { for(int y=0; y < h; y++) { // Fetch the normal map normal vector Color c = normalMap.GetPixel(x, y); Vector3 normal = new Vector3(colorToFloat(c.R), colorToFloat(c.G), colorToFloat(c.B)).ToNormalized(); Vector3 result = normal; // If we're at the edge, use the normal vector if (x < w - 1 && y < h - 1) { // Compute the bump normal vector int xyLevel = bumpLevel(bumpMap, x, y); float dx = scaleFactor * (bumpLevel(bumpMap, x+1, y) - xyLevel); float dy = scaleFactor * (bumpLevel(bumpMap, x, y+1) - xyLevel); float dz = 255f; Vector3 bumpNormal = new Vector3(dx, dy, dz).ToNormalized(); if (generateLogFile.Checked) Log("X {0}, Y {1}, normal {2}, bumpNormal {3}\n", x, y, normal, bumpNormal); Vector3 axis = unitZ.Cross(normal); if (axis.Length > epsilon) { float cosAngle = unitZ.Dot(normal); float angle = (float)Math.Acos(cosAngle); Quaternion q = Quaternion.FromAngleAxis(angle, axis); Matrix3 rot = q.ToRotationMatrix(); result = rot * bumpNormal; if (generateLogFile.Checked) Log(" Angle {0}, Quaternion {1}, Result {2}\n", angle, q, result); } } Color resultColor = Color.FromArgb(floatToColor(result.x), floatToColor(result.y), floatToColor(result.z)); outputMap.SetPixel(x, y, resultColor); } } if (generateLogFile.Checked) logStream.Close(); outputMap.Save(outputMapTextBox.Text); outputPictureBox.Image = outputMap; outputPictureBox.Visible = true; Cursor.Current = previousCursor; doneLabel.Visible = 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]; } }
internal GridPolygon(CellStatus status, GridCell c1, GridCell c2, GridCell c3, GridCell c4, Vector3 c1Loc, Vector3 c2Loc, Vector3 c3Loc, Vector3 c4Loc) { this.status = status; corners = new GridCell[] { c1, c2, c3, c4 }; cornerLocs = new Vector3[] { c1Loc, c2Loc, c3Loc, c4Loc }; }
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 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); }
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); } } }