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));
}
// 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 bool ShapeCollides(CollisionShape shape, CollisionParms parms)
{
collisionTimeStamp++;
SphereTreeNode s = sphereTree.FindSmallestContainer(shape, null);
return(s.TestSphereCollision(shape, collisionTimeStamp, ref collisionTestCount, parms));
}
public bool ShapeCollides(CollisionShape shape)
{
CollisionParms parms = new CollisionParms();
parms.genNormals = false;
return(ShapeCollides(shape, parms));
}
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);
}
// Test for collision with any object in this sphere
public void FindIntersectingShapes(CollisionShape part,
List <CollisionShape> shapes,
ulong timeStamp,
ref int collisionTestCount,
CollisionParms parms)
{
if (containedShape != null)
{
collisionTestCount++;
parms.swapped = false;
if (Primitives.TestCollisionFunctions[(int)part.ShapeType(),
(int)containedShape.ShapeType()]
(part, containedShape, parms))
{
shapes.Add(containedShape);
}
else
{
containedShape.timeStamp = timeStamp;
}
}
// Iterate over the shapes in this sphere and not wholly
// contained in a subsphere
foreach (CollisionShape obstacle in intersectingShapes)
{
if (obstacle.timeStamp == timeStamp)
{
continue;
}
collisionTestCount++;
parms.swapped = false;
if (Primitives.TestCollisionFunctions[(int)part.ShapeType(), (int)obstacle.ShapeType()]
(part, obstacle, parms))
{
shapes.Add(obstacle);
}
else
{
obstacle.timeStamp = timeStamp;
}
}
// Now iterate over subspheres
for (int i = 0; i < SphereTreeNode.childCount; i++)
{
SphereTreeNode cs = children[i];
if (cs == null)
{
continue;
}
// Skip any sphere that doesn't overlap the part in question
if (!cs.SphereOverlap(part))
{
continue;
}
cs.FindIntersectingShapes(part, shapes, timeStamp, ref collisionTestCount, parms);
}
}
public CollisionShape FindClosestCollision(CollisionShape cap, Vector3 start, Vector3 end,
ref Vector3 intersection)
{
collisionTimeStamp++;
SphereTreeNode s = sphereTree.FindSmallestContainer(cap, null);
CollisionParms parms = new CollisionParms();
parms.genNormals = false;
List <CollisionShape> shapes = new List <CollisionShape>();
s.FindIntersectingShapes(cap, shapes, collisionTimeStamp, ref collisionTestCount, parms);
intersection = Vector3.Zero;
if (shapes.Count == 0)
{
return(null);
}
else
{
collisionTimeStamp++;
return(FindClosestIntersectingShape(shapes, start, end, ref intersection));
}
}
public static bool TestCollisionSphereOBB(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
CollisionSphere x1 = (CollisionSphere)s1;
CollisionOBB x2 = (CollisionOBB)s2;
Vector3 closest;
if (TestSphereOBB(x1, x2, out closest)) {
Vector3 n = s1.center - closest;
parms.SetNormPart(n);
// ??? This isn't the correct value of the normal
parms.SetNormObstacle(-n);
return true;
}
else
return false;
}
//////////////////////////////////////////////////////////////////////
//
// The per-shape-pair collision predicates. Since collisions
// themselves are much less frequent than the tests for
// collisions, all the effort is in making the tests fast, and
// if we have to repeat the same calculations in the event of
// a collision, it's still a net savings if it makes the tests
// cheaper
//
//////////////////////////////////////////////////////////////////////
public static bool TestCollisionSphereSphere(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
CollisionSphere x1 = (CollisionSphere)s1;
CollisionSphere x2 = (CollisionSphere)s2;
Vector3 nS2 = s2.center - s1.center;
if (x1.radius + x2.radius > nS2.Length) {
Vector3 n = s2.center - s1.center;
parms.SetNormPart(n);
parms.SetNormObstacle(-n);
return true;
}
else
return false;
}
// Test for collision with any object in this sphere
public bool TestSphereCollision(CollisionShape part, ulong timeStamp,
ref int collisionTestCount, CollisionParms parms)
{
if (containedShape != null) {
collisionTestCount++;
parms.swapped = false;
if (Primitives.TestCollisionFunctions[(int)part.ShapeType(),
(int)containedShape.ShapeType()]
(part, containedShape, parms)) {
parms.part = part;
parms.obstacle = containedShape;
return true;
}
else {
containedShape.timeStamp = timeStamp;
}
}
// Iterate over the shapes in this sphere and not wholly
// contained in a subsphere
foreach (CollisionShape obstacle in intersectingShapes) {
if (obstacle.timeStamp == timeStamp)
continue;
collisionTestCount++;
parms.swapped = false;
if (Primitives.TestCollisionFunctions[(int)part.ShapeType(), (int)obstacle.ShapeType()]
(part, obstacle, parms)) {
parms.part = part;
parms.obstacle = obstacle;
return true;
}
else {
obstacle.timeStamp = timeStamp;
}
}
// Now iterate over subspheres
for (int i=0; i<SphereTreeNode.childCount; i++) {
SphereTreeNode cs = children[i];
if (cs == null)
continue;
// Skip any sphere that doesn't overlap the part in question
if (!cs.SphereOverlap(part))
continue;
if (cs.TestSphereCollision(part, timeStamp, ref collisionTestCount, parms))
return true;
}
return false;
}
public static bool TestCollisionSphereCapsule(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
CollisionSphere x1 = (CollisionSphere)s1;
CollisionCapsule x2 = (CollisionCapsule)s2;
float rSum = x1.radius + x2.capRadius;
if (SqDistPointSegment(x2.bottomcenter, x2.topcenter, x1.center) < rSum * rSum)
{
float t;
Vector3 d;
ClosestPtPointSegment(x1.center, x2.bottomcenter, x2.topcenter, out t, out d);
Vector3 n = d - x1.center;
parms.SetNormPart(n);
parms.SetNormObstacle(-n);
return true;
}
else
return false;
}
// 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;
}
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;
}
// The is the top-level collision detection function.
//
// The MovingObject doesn't intersect anything now; would it
// do so if the vector displacement is applied?
//
// If we do have a collision, we "creep up" on the object we collide
// with until we're within stepsize of the minimum dimension of the
// moving part
//
public bool TestCollision(MovingObject mo, float stepSize, ref Vector3 displacement, CollisionParms parms)
{
topLevelCalls++;
parms.Initialize();
if (mo.parts.Count == 0)
return false;
// Remember where the first part started
Vector3 start = mo.parts[0].shape.center;
Vector3 originalDisplacement = displacement;
float len = displacement.Length;
//MaybeDumpSphereTree();
int nsteps = (int)Math.Ceiling(len / stepSize);
Vector3 step = (len <= stepSize ? displacement : displacement * (stepSize/len));
// Try to step the whole way
if (!StepTowardCollision(mo, displacement, nsteps, step, parms)) {
displacement = Vector3.Zero;
return false;
}
// Otherwise, we hit something. Step toward it
// The minimum distance
float smallStepSize = Math.Min(MO.InchesToMeters(MinInchesToObstacle),
stepSize * MinFractionToObstacle);
len = step.Length;
nsteps = (int)Math.Ceiling(len/smallStepSize);
Vector3 smallStep = step * (smallStepSize/len);
bool hit = StepTowardCollision(mo, step, nsteps, smallStep, parms);
displacement = originalDisplacement - (mo.parts[0].shape.center - start);
return hit;
}
public bool ShapeCollides(CollisionShape shape, CollisionParms parms)
{
collisionTimeStamp++;
SphereTreeNode s = sphereTree.FindSmallestContainer(shape, null);
return s.TestSphereCollision(shape, collisionTimeStamp, ref collisionTestCount, parms);
}
public static bool TestCapsuleCapsule(CollisionCapsule c1, CollisionCapsule c2,
CollisionParms parms)
{
// Compute (squared) distance between the inner structures of the capsules
float s, t;
Vector3 p1, p2;
float d = ClosestPtSegmentSegment(c1.bottomcenter, c1.topcenter,
c2.bottomcenter, c2.topcenter,
out s, out t, out p1, out p2);
float r = c1.capRadius + c2.capRadius;
Vector3 n = p2 - p1;
parms.SetNormPart(n);
parms.SetNormObstacle(-n);
return d < r * r;
}
public static bool TestCapsuleOBB(CollisionCapsule a, CollisionOBB b, CollisionParms parms)
{
SegOBBParams obbParms = new SegOBBParams(true);
Segment s = Segment.SegmentFromStartAndEnd(a.bottomcenter, a.topcenter);
if (SqrDistSegOBB(s, b, obbParms) < a.capRadius * a.capRadius) {
// If parms.pfLParam == 0, closest is bottomcenter; if == 1,
// closest is topcenter.
Vector3 d = a.bottomcenter + obbParms.pfLParam * s.direction;
// pfBVec is relative to the center of the box, but in box
// coords.
Vector3 f = b.center;
for (int i=0; i<3; i++)
f += obbParms.pfBVec[i] * b.axes[i];
Vector3 n = f - d;
parms.SetNormPart(n);
parms.SetNormObstacle(-n);
if (MO.DoLog) {
MO.Log(" TestCapsuleOBB: pfLParam {0}, pfBVec {1}", obbParms.pfLParam, obbParms.pfBVec);
MO.Log(" TestCapsuleOBB: d {0}, f {1}", f, d);
MO.Log(" -n {0}", -n);
}
return true;
}
else
return false;
}
public static 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);
}
private void MoveToObject(StreamWriter stream,
CollisionAPI API, CollisionShape collisionShape,
CollisionShape movingShape)
{
stream.Write("\n\n\nEntering MoveToObject\n");
// Create a MovingObject, and add movingShape to it
MovingObject mo = new MovingObject();
API.AddPartToMovingObject(mo, movingShape);
// Move movingObject 1 foot at a time toward the sphere
Vector3 toShape = collisionShape.center - movingShape.center;
stream.Write(string.Format("movingShape {0}\n", movingShape.ToString()));
stream.Write(string.Format("collisionShape {0}\nDisplacement Vector {1}\n",
collisionShape.ToString(), toShape.ToString()));
// We'll certainly get there before steps expires
int steps = (int)Math.Ceiling(toShape.Length);
// 1 foot step in the right direction
Vector3 stepVector = toShape.ToNormalized();
stream.Write(string.Format("Steps {0}, stepVector {1}\n", steps, stepVector.ToString()));
bool hitIt = false;
// Loop til we smack into something
CollisionParms parms = new CollisionParms();
for (int i=0; i<steps; i++) {
// Move 1 foot; if returns true, we hit something
hitIt = (API.TestCollision (mo, stepVector, parms));
stream.Write(string.Format("i = {0}, hitIt = {1}, movingShape.center = {2}\n",
i, hitIt, movingShape.center.ToString()));
if (hitIt) {
stream.Write(string.Format("collidingPart {0}\nblockingObstacle {1}\n, normPart {2}, normObstacle {3}\n",
parms.part.ToString(), parms.obstacle.ToString(),
parms.normPart.ToString(), parms.normObstacle.ToString()));
return;
}
stream.Write("\n");
}
Debug.Assert(hitIt, "Didn't hit the obstacle");
}
// Keep the code below, commented out, because it is useful
// for debugging near plane issues
// private static int nearPlaneCollisionCount = 0;
// private static int nearPlaneNodeId = 99999;
// private static List<RenderedNode>[] nearPlaneRenderedNodes = new List<RenderedNode>[4] {
// new List<RenderedNode>(), new List<RenderedNode>(), new List<RenderedNode>(), new List<RenderedNode>() };
// private static CollisionCapsule[] nearPlaneCapsules = new CollisionCapsule[4];
//
// protected void CreateNearPlaneCapsules(Camera camera,
// Vector3 cameraPosition,
// Vector3 playerHeadPosition)
// {
// // Frame the clip plane with 4 narrow capsules
// // Start by finding the center of the near plane
// Vector3 toPlayer = (playerHeadPosition - cameraPosition).ToNormalized();
// Vector3 center = cameraPosition + (1.02f * camera.Near) * toPlayer;
// float thetaY = MathUtil.DegreesToRadians(camera.FOVy * 0.5f);
// float tanThetaY = MathUtil.Tan(thetaY);
// float tanThetaX = tanThetaY * camera.AspectRatio;
// Vector3 right = tanThetaX * camera.Near * (camera.Right.ToNormalized());
// Vector3 up = tanThetaY * camera.Near * (camera.Up.ToNormalized());
// Vector3 corner1 = center + right - up;
// Vector3 corner2 = center + right + up;
// Vector3 corner3 = center - right + up;
// Vector3 corner4 = center - right - up;
// nearPlaneCapsules[0] = new CollisionCapsule(corner1, corner2, Client.OneMeter * .001f);
// nearPlaneCapsules[1] = new CollisionCapsule(corner2, corner3, Client.OneMeter * .001f);
// nearPlaneCapsules[2] = new CollisionCapsule(corner3, corner4, Client.OneMeter * .001f);
// nearPlaneCapsules[3] = new CollisionCapsule(corner4, corner1, Client.OneMeter * .001f);
//
// RenderedNode.Scene = sceneManager;
// for (int i=0; i<4; i++) {
// RenderedNode.RemoveNodeRendering(nearPlaneNodeId + i, ref nearPlaneRenderedNodes[i]);
// RenderedNode.CreateRenderedNodes(nearPlaneCapsules[i], nearPlaneNodeId + i, ref nearPlaneRenderedNodes[i]);
// }
// }
protected float FindAcceptableCameraPosition(Camera camera, Vector3 cameraPos, Vector3 cameraTarget)
{
//Logger.Log(0, "FindAcceptableCameraPosition cameraPos {0}, cameraTarget {1}", cameraPos, cameraTarget);
// If there is no CollisionAPI object, punt
CollisionAPI api = client.WorldManager.CollisionHelper;
if (api == null)
return (cameraPos - cameraTarget).Length;
// Start by finding the point at which a narrow capsule
// from the camera to the player intersects. If it
// doesn't, then set the point to the cameraPos.
// Make a very narrow capsule between the camera and the player's head
CollisionCapsule cap = new CollisionCapsule(cameraPos,
cameraTarget,
Client.OneMeter * .001f); // 1 millimeter
Vector3 newCameraPos = cameraPos;
Vector3 unitTowardCamera = (newCameraPos - cameraTarget).ToNormalized();
// Test for a collision, setting intersection if there is one.
Vector3 intersection = Vector3.Zero;
if (api.FindClosestCollision(cap, cameraTarget, cameraPos, ref intersection) != null) {
// There is A collision - - set the new camera pos to
// be the point of intersection
newCameraPos = intersection - unitTowardCamera * camera.Near;
//Logger.Log(0, "FindAcceptableCameraPosition: Thin cap collides at {0}", newCameraPos);
}
// Move a near-plane OBB forward from the point of
// intersection until we find a place where the near plane
// doesn't intersect any collision volumes
Vector3 center = newCameraPos - camera.Near * unitTowardCamera;
float thetaY = MathUtil.DegreesToRadians(camera.FOVy * 0.5f);
float tanThetaY = MathUtil.Tan(thetaY);
float tanThetaX = tanThetaY * camera.AspectRatio;
// Compute the OBB axes and extents
Vector3[] axes = new Vector3[3];
Vector3 extents = Vector3.Zero;
// The axis perpendicular to the near plane
float obbDepth = 100f; // 100 mm thick
axes[0] = -unitTowardCamera;
axes[1] = -axes[0].Cross(Vector3.UnitY).ToNormalized();
axes[2] = -axes[1].Cross(axes[0]).ToNormalized();
extents[0] = obbDepth;
extents[1] = camera.Near * tanThetaX;
extents[2] = camera.Near * tanThetaY;
float len = (newCameraPos - cameraTarget).Length;
float startingLen = len;
CollisionOBB obb = new CollisionOBB(center, axes, extents);
CollisionParms parms = new CollisionParms();
while (len >= minThirdPersonDistance) {
obb.center = cameraTarget + unitTowardCamera * len;
bool collides = api.ShapeCollides(obb, parms);
//Logger.Log(0, "FindAcceptableCameraPosition len {0}, obb center {1}, collides {2}", len, obb.center, collides);
if (!collides)
break;
//client.Write(string.Format("OBB collides; len {0}", len));
len -= obbDepth;
}
// Len is less than the camera min distance, so just
// return it
//Logger.Log(0, "FindAcceptableCameraPosition: starting len {0} final len {1}", startingLen, len);
return len;
}
// This function swaps the arguments so we only have to
// implement the intersection function once
public static bool TestCollisionSwapper(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
parms.swapped = true;
return TestCollisionFunctions[(int)s2.ShapeType(), (int)s1.ShapeType()]
(s2, s1, parms);
}
public bool ShapeCollides(CollisionShape shape)
{
CollisionParms parms = new CollisionParms();
parms.genNormals = false;
return ShapeCollides(shape, parms);
}
public static bool TestOBBOBB(CollisionOBB a, CollisionOBB b, CollisionParms parms)
{
if (MO.DoLog)
MO.Log("TestOBBOBB Entering: a {0} b {1}", a, b);
if (TestOBBOBBInternal(a, b)) {
Vector3 n = b.center - a.center;
// ??? Not right
parms.SetNormPart(n);
parms.SetNormObstacle(-n);
if (MO.DoLog)
MO.Log("TestOBBOBB Collided: n {0}", n);
return true;
}
else
return false;
}
// We only call this if both the collisionManager and the collider exist
private static Vector3 FindMobNodeDisplacement(MobNode mobNode, CollisionParms parms, Vector3 desiredDisplacement, out bool collided)
{
Vector3 start = mobNode.Position;
Vector3 pos = start + desiredDisplacement;
Vector3 displacement = desiredDisplacement;
MovingObject mo = mobNode.Collider;
Vector3 moStart = mo.parts[0].shape.center;
if (MO.DoLog) {
MO.Log(" moStart = {0}, start = {1}", moStart, start);
MO.Log(" pos = {0}, displacement = {1}", pos, displacement);
TraceMOBottom(mo, " On entry to FindMobNodeDisplacement");
}
collided = false;
if (collisionManager.TestCollision(mo, ref displacement, parms)) {
collided = true;
if (MO.DoLog) {
MO.Log(" Collision when moving object {0} from {1} to {2}",
parms.part.handle, start, pos);
NowColliding(mo, " After first TestCollision in FindMobNodeDisplacement");
TraceObstacle(parms.obstacle);
MO.Log(" Before collision moved {0}", desiredDisplacement - displacement);
}
// Decide if the normals are such that we want
// to slide along the obstacle
Vector3 remainingDisplacement = displacement;
Vector3 norm1 = parms.normObstacle.ToNormalized();
if (DecideToSlide(mo, start + displacement, parms, ref remainingDisplacement)) {
if (MO.DoLog)
MO.Log(" After DecideToSlide, remainingDisplacement {0}", remainingDisplacement);
// We have to test the displacement
if (collisionManager.TestCollision(mo, ref remainingDisplacement, parms)) {
if (MO.DoLog) {
NowColliding(mo, " After first try TestCollision");
MO.Log(" Slid into obstacle on the first try; remainingDisplacement = {0}",
remainingDisplacement);
TraceObstacle(parms.obstacle);
}
if (remainingDisplacement.LengthSquared > 0) {
Vector3 norm2 = parms.normObstacle.ToNormalized();
// Find the cross product of the of norm1 and
// norm2, and dot with displacement. If
// negative, reverse.
Vector3 newDir = norm1.Cross(norm2);
float len = newDir.Dot(remainingDisplacement);
if (len < 0) {
newDir = - newDir;
len = - len;
}
Vector3 slidingDisplacement = len * newDir;
Vector3 originalSlidingDisplacement = slidingDisplacement;
if (MO.DoLog) {
MO.Log(" norm1 = {0}, norm2 = {1}, len = {2}",
norm1, norm2, len);
MO.Log(" Cross product slidingDisplacement is {0}", slidingDisplacement);
}
if (collisionManager.TestCollision(mo, ref slidingDisplacement, parms)) {
if (MO.DoLog) {
NowColliding(mo, " After second try TestCollision");
MO.Log(" Slid into obstacle on the second try; slidingDisplacement = {0}",
slidingDisplacement);
}
}
else
if (MO.DoLog)
MO.Log(" Didn't slide into obstacle on the second try");
remainingDisplacement -= (originalSlidingDisplacement - slidingDisplacement);
}
}
}
else
remainingDisplacement = displacement;
if (MO.DoLog)
MO.Log(" Before checking hop, remainingDisplacement is {0}", remainingDisplacement);
if (remainingDisplacement.Length > 30f) {
// Try to hop over the obstacle
Vector3 c = remainingDisplacement;
mo.AddDisplacement(new Vector3(0f, CollisionAPI.HopOverThreshold * Client.OneMeter, 0f));
if (MO.DoLog) {
TraceMOBottom(mo, " Before trying to hop");
MO.Log(" remainingDisplacement {0}", remainingDisplacement);
}
if (collisionManager.TestCollision(mo, ref remainingDisplacement, parms)) {
if (MO.DoLog) {
MO.Log(" Even after hopping up {0} meters, can't get over obstacle; disp {1}",
CollisionAPI.HopOverThreshold, remainingDisplacement);
}
c = c - remainingDisplacement;
c.y = 0;
c += new Vector3(0f, CollisionAPI.HopOverThreshold * Client.OneMeter, 0f);
if (MO.DoLog)
MO.Log(" After failed hop, subtracting {0}", c);
mo.AddDisplacement(- c);
}
else if (MO.DoLog) {
MO.Log(" Hopping up {0} meters got us over obstacle; disp {1}",
CollisionAPI.HopOverThreshold, remainingDisplacement);
TraceMOBottom(mo, " After hopping");
}
NowColliding(mo, " After hopping");
}
}
Vector3 moPos = mo.parts[0].shape.center;
pos = start + moPos - moStart;
if (MO.DoLog) {
MO.Log(" mo location = {0}, moPos - moStart {1}", moPos, moPos - moStart);
NowColliding(mo, " Leaving FindMobNodeDisplacement");
MO.Log(" pos = {0}", pos);
}
return pos;
}
public CollisionShape FindClosestCollision(CollisionShape cap, Vector3 start, Vector3 end,
ref Vector3 intersection)
{
collisionTimeStamp++;
SphereTreeNode s = sphereTree.FindSmallestContainer(cap, null);
CollisionParms parms = new CollisionParms();
parms.genNormals = false;
List<CollisionShape> shapes = new List<CollisionShape>();
s.FindIntersectingShapes(cap, shapes, collisionTimeStamp, ref collisionTestCount, parms);
intersection = Vector3.Zero;
if (shapes.Count == 0)
return null;
else {
collisionTimeStamp++;
return FindClosestIntersectingShape(shapes, start, end, ref intersection);
}
}
// Move the desired displacement, limited by hitting an
// obstacle. Then, if we're not already at the terrain level,
// "fall" until we are either at the terrain level, or hit an
// obstacle
public static Vector3 MoveMobNode(MobNode mobNode, Vector3 requestedDisplacement, Client client)
{
// Logger.Log(0, "MoveMobNode oid {0} requestedDisplacement {1}", mobNode.Oid, requestedDisplacement);
// log.DebugFormat("MoveMobNode: mobNode oid {0}, name {1}, followTerrain {2}, position {3}, disp {4}",
// mobNode.Oid, mobNode.Name, mobNode.FollowTerrain, mobNode.Position, requestedDisplacement);
Vector3 start = mobNode.Position;
MovingObject mo = mobNode.Collider;
bool collided = false;
// Zero the y coordinate of displacement, because it seems
// that it can be arbitrarily large
Vector3 desiredDisplacement = requestedDisplacement;
if (mobNode.FollowTerrain)
desiredDisplacement.y = 0;
if (desiredDisplacement.LengthSquared <= float.Epsilon)
return start;
if (MO.DoLog)
MO.Log("MoveMobNode called with mobNode {0} at {1}, disp of {2}",
mobNode.Oid, start, requestedDisplacement);
if (collisionManager == null) {
log.Info("MoveMobNode: returning because collisionManager isn't initialized");
return start + desiredDisplacement;
}
if (mo == null || mo.parts.Count == 0) {
if (MO.DoLog)
MO.Log("MoveMobNode returning because no collision volume for node");
return start + requestedDisplacement;
}
if (mobNode is Player && NowColliding(mo, "Testing player collision on entry")) {
if (client.MillisecondsStuckBeforeGotoStuck != 0) {
if (!playerStuck) {
stuckGotoTime = DateTime.Now.AddMilliseconds(client.MillisecondsStuckBeforeGotoStuck);
playerStuck = true;
}
else if (DateTime.Now >= stuckGotoTime) {
// We issue the goto command to move us out of the
// collision volume
client.Write("Executing /stuck command because player has been in a collision volume for " + client.MillisecondsStuckBeforeGotoStuck + " milliseconds");
client.NetworkHelper.SendTargettedCommand(client.Player.Oid, "/stuck");
playerStuck = false;
return start;
}
}
}
else
playerStuck = false;
// If we haven't completed setup to this extent, just give up
CollisionParms parms = new CollisionParms();
Vector3 pos = FindMobNodeDisplacement(mobNode, parms, desiredDisplacement, out collided);
// log.DebugFormat("MoveMobNode: mobNode oid {0}, name {1}, mob node position {2}, displacement {3}",
// mobNode.Oid, mobNode.Name, pos, requestedDisplacement);
float h = worldManager.GetHeightAt(pos);
// If we're already below ground level, just set our
// level to ground level. This will have to be modified
// if we deal with caves
if (pos.y - h < 0) {
// log.DebugFormat("MoveMobNode: mobNode oid {0}, name {1} below terrain level", mobNode.Oid, mobNode.Name);
mo.AddDisplacement(new Vector3(0f, h - pos.y, 0f));
pos.y = h;
if (MO.DoLog && (pos.y - h) < -.001 * Client.OneMeter)
MO.Log(string.Format(" MobNode at {0} is below ground height {1}!",
pos, h));
} // else {
if (mobNode.FollowTerrain) {
// NowColliding(mo, " Before falling loop");
// Fall toward the terrain or an obstacle, whichever comes
// first
float step = mo.StepSize(new Vector3(0, h, 0));
while (true) {
if (Math.Abs(pos.y - h) < CollisionAPI.VerticalTerrainThreshold * Client.OneMeter) {
mo.AddDisplacement(new Vector3(0f, h - pos.y, 0f));
pos.y = h;
break;
} else {
float dy = -Math.Min(pos.y - h, step);
Vector3 displacement = new Vector3(0, dy, 0);
Vector3 cd = displacement;
if (MO.DoLog) {
MO.Log(" Testing for collision falling {0}", dy);
TraceMOBottom(mo, " Before falling");
}
if (collisionManager.TestCollision(mo, ref displacement, parms)) {
if (MO.DoLog) {
TraceMOBottom(mo, " After TestCollision after falling");
NowColliding(mo, " After TestCollision after falling");
MO.Log(" Collision when object {0} falls from {1} to {2}",
parms.part.handle, pos, pos + cd);
TraceObstacle(parms.obstacle);
MO.Log(" Adding dy {0} - displacement.y {1} to pos {2}",
dy, displacement.y, pos);
}
pos.y += dy - displacement.y;
break;
}
if (MO.DoLog)
MO.Log(" Didn't collide falling; dy {0}, pos {1}",
dy, pos);
pos.y += dy;
}
}
} else {
if (MO.DoLog)
MO.Log(" Not falling because mobNode {0} doesn't have FollowTerrain",
mobNode.Oid);
}
// }
if (MO.DoLog) {
NowColliding(mo, " Leaving MoveMobNode");
MO.Log("MoveMobNode returning pos {0}", pos);
MO.Log("");
}
if (collided)
log.DebugFormat("MoveMobNode collided: mobNode oid {0}, name {1}, orig pos {2}, displacement {3}, new pos {4}",
mobNode.Oid, mobNode.Name, start, requestedDisplacement, pos);
else
log.DebugFormat("MoveMobNode didn't collide: mobNode oid {0}, name {1}, orig pos {2}, displacement {3}, new pos {4}",
mobNode.Oid, mobNode.Name, start, requestedDisplacement, pos);
return pos;
}
public static bool TestCollisionAABBAABB(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
CollisionAABB x1 = (CollisionAABB)s1;
CollisionAABB x2 = (CollisionAABB)s2;
if (TestAABBAABB(x1, x2)) {
Vector3 n = x2.center - x1.center;
parms.SetNormPart(n);
// ??? This isn't the correct value of the normal
parms.SetNormObstacle(-n);
return true;
}
else
return false;
}
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);
}
// The is the top-level collision detection function.
//
// The MovingObject doesn't intersect anything now; would it
// do so if the vector displacement is applied?
//
// If we do have a collision, we "creep up" on the object we collide
// with until we're within stepsize of the minimum dimension of the
// moving part
//
public bool TestCollision(MovingObject mo, float stepSize, ref Vector3 displacement, CollisionParms parms)
{
topLevelCalls++;
parms.Initialize();
if (mo.parts.Count == 0)
{
return(false);
}
// Remember where the first part started
Vector3 start = mo.parts[0].shape.center;
Vector3 originalDisplacement = displacement;
float len = displacement.Length;
//MaybeDumpSphereTree();
int nsteps = (int)Math.Ceiling(len / stepSize);
Vector3 step = (len <= stepSize ? displacement : displacement * (stepSize / len));
// Try to step the whole way
if (!StepTowardCollision(mo, displacement, nsteps, step, parms))
{
displacement = Vector3.Zero;
return(false);
}
// Otherwise, we hit something. Step toward it
// The minimum distance
float smallStepSize = Math.Min(MO.InchesToMeters(MinInchesToObstacle),
stepSize * MinFractionToObstacle);
len = step.Length;
nsteps = (int)Math.Ceiling(len / smallStepSize);
Vector3 smallStep = step * (smallStepSize / len);
bool hit = StepTowardCollision(mo, step, nsteps, smallStep, parms);
displacement = originalDisplacement - (mo.parts[0].shape.center - start);
return(hit);
}
public static bool TestCollisionAABBOBB(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
CollisionAABB x1 = (CollisionAABB)s1;
CollisionOBB x2 = (CollisionOBB)s2;
// Convert the AABB into an OBB, then run the OBBOBB test.
// Not the most efficient algorithm but it gets the job
// done.
CollisionOBB newx1 = new CollisionOBB(x1.center,
UnitBasisVectors,
(x1.max - x1.min) * .5f);
return TestOBBOBB(newx1, x2, parms);
}
// Decide, based on the normal to the collision object, if the
// moving object can slide across the obstacle, and if it can,
// return the updated displacement. This displacement may in fact
// run into _another_ obstacle, however, so the call must again
// run the collision test.
private static bool DecideToSlide(MovingObject mo, Vector3 mobNodePosition,
CollisionParms parms, ref Vector3 displacement)
{
Vector3 normDisplacement = displacement.ToNormalized();
Vector3 normObstacle = parms.normObstacle.ToNormalized();
if (MO.DoLog) {
MO.Log(" DecideToSlide: normObstacle {0}, normDisplacement {1}",
normObstacle.ToString(), normDisplacement.ToString());
MO.Log(" DecideToSlide: displacement {0}", displacement);
}
// First we find the angle between the normal and the
// direction of travel, and reject the displacement if
// it's too small
float slideAngle = (NormalizeAngle((float)Math.Acos((double)normDisplacement.Dot(normObstacle))) -
.5f * (float)Math.PI);
if (Math.Abs(slideAngle) > CollisionAPI.MinSlideAngle) {
if (MO.DoLog)
MO.Log(" After collision, displacement {0}, won't slide because slideAngle {1} > minSlideAngle {2}",
displacement.ToString(), slideAngle, CollisionAPI.MinSlideAngle);
displacement = Vector3.Zero;
return false;
}
// Then we find the angle with the y axis, and reject the
// displacement if it's too steep
float verticalAngle = NormalizeAngle((float)Math.Acos((double)normDisplacement[1]));
if (Math.Abs(verticalAngle) > CollisionAPI.MaxVerticalAngle) {
if (MO.DoLog)
MO.Log(" After collision, displacement {0}, won't slide because verticalAngle {1} <= maxVerticalAngle {2}",
displacement.ToString(), verticalAngle, CollisionAPI.MaxVerticalAngle);
displacement = Vector3.Zero;
return false;
}
// Else, we can slide, so return a displacement that
// points in the direction we're sliding, and has length
// equal to a constant times the displacement length
// Rotate displacement so that it's 90 degress from the
// obstacle normal
Vector3 cross = normObstacle.Cross(normDisplacement);
Quaternion q = Quaternion.FromAngleAxis(.5f * (float)Math.PI, cross);
Matrix4 transform = q.ToRotationMatrix();
Vector3 transformedNorm = transform * normObstacle.ToNormalized();
float len = displacement.Length;
displacement = transformedNorm * len;
// Vector3 alignedPart = normObstacle * (normObstacle.Dot(displacement));
// displacement -= alignedPart;
Vector3 p = mobNodePosition + displacement;
float h = worldManager.GetHeightAt(p);
// If sliding would put us below ground, limit the displacement
if (h > p.y) {
if (MO.DoLog)
MO.Log(" Sliding up because terrain height is {0} is higher than projected mobNode height {1}",
h, p.y);
displacement.y += h - p.y;
}
if (MO.DoLog) {
MO.Log(" Exiting DecideToSlide, sliding displacement {0}, slideAngle {1}, verticalAngle {2}",
displacement.ToString(), slideAngle, verticalAngle);
MO.Log(" Exiting DecideToSlide, cross product {0}, quaternion {1}, transformedNorm {2}",
cross, q, transformedNorm);
// MO.Log(" Exiting DecideToSlide, alignedPart {0}", alignedPart);
}
return true;
}
public static bool TestCollisionCapsuleCapsule(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
CollisionCapsule x1 = (CollisionCapsule)s1;
CollisionCapsule x2 = (CollisionCapsule)s2;
return TestCapsuleCapsule(x1, x2, parms);
}
private static bool NowColliding(MovingObject mo, string description)
{
CollisionParms parms = new CollisionParms();
bool colliding = collisionManager.CollideAfterAddingDisplacement(mo, Vector3.Zero, parms);
CollisionShape obstacle = mo.parts[0].shape;
CollisionCapsule moCapsule = null;
if (obstacle is CollisionCapsule)
moCapsule = (CollisionCapsule)obstacle;
string rest = (moCapsule != null ?
string.Format("mo bottom is {0}",
moCapsule.bottomcenter - new Vector3(0f, moCapsule.capRadius, 0f)) :
string.Format("obstacle {0}", obstacle));
if (MO.DoLog)
MO.Log("{0}, now colliding = {1}; {2}", description, colliding, rest);
log.DebugFormat("{0}, now colliding = {1}; {2}", description, colliding, rest);
return colliding;
}
public static bool TestCollisionOBBOBB(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
CollisionOBB x1 = (CollisionOBB)s1;
CollisionOBB x2 = (CollisionOBB)s2;
return TestOBBOBB(x1, x2, parms);
}
///<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 static bool TestCollisionSphereAABB(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
CollisionSphere x1 = (CollisionSphere)s1;
CollisionAABB x2 = (CollisionAABB)s2;
float d = SqDistPointAABB(x1.center, x2);
if (d < Square(x1.radius)) {
Vector3 n;
ClosestPtPointAABB(x1.center, x2, out n);
n -= x1.center;
parms.SetNormPart(n);
// ??? This isn't the correct value of the normal
parms.SetNormObstacle(-n);
return true;
}
else
return false;
}