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