public Plane(Vector3 first, Vector3 second, Vector3 third, Texture2D texture, double hO, double vO, double tR, double hS, double vS)
{
//a plane is defined by three points. we want the second of the points to be the corner so that first -> second -> third goes along
//two edges of the plane
this.first = MapEngine.toWorldSpace(first);
this.second = MapEngine.toWorldSpace(second);
this.third = MapEngine.toWorldSpace(third);
meshFirst = this.first;
meshSecond = this.second;
meshThird = this.third;
this.texture = texture;
plane = new Microsoft.Xna.Framework.Plane(this.first, this.second, this.third);
//this accounts for a different winding from XNA
plane.Normal = -plane.Normal;
fixMeshOrder();
hOffset = hO;
vOffset = vO;
textureRotation = tR;
hScale = hS;
vScale = vS;
}
public Plane(Vector3 first, Vector3 second, Vector3 third, Texture2D texture, double hO, double vO, double tR, double hS, double vS)
{
//a plane is defined by three points. we want the second of the points to be the corner so that first -> second -> third goes along
//two edges of the plane
this.first = MapEngine.toWorldSpace(first);
this.second = MapEngine.toWorldSpace(second);
this.third = MapEngine.toWorldSpace(third);
meshFirst = this.first;
meshSecond = this.second;
meshThird = this.third;
this.texture = texture;
plane = new Microsoft.Xna.Framework.Plane(this.first, this.second, this.third);
//this accounts for a different winding from XNA
plane.Normal = -plane.Normal;
fixMeshOrder();
hOffset = hO;
vOffset = vO;
textureRotation = tR;
hScale = hS;
vScale = vS;
}
/// <summary>
/// Set the indices into the relevant vertex array for this
/// triangle. Also sets the plane and bounding box
/// </summary>
/// <param name="i0"></param>
/// <param name="i1"></param>
/// <param name="i2"></param>
/// <param name="vertexArray"></param>
public void SetVertexIndices(int i0, int i1, int i2, Vector3[] vertexArray)
{
vertexIndices0 = i0;
vertexIndices1 = i1;
vertexIndices2 = i2;
plane = new Microsoft.Xna.Framework.Plane(vertexArray[i0], vertexArray[i1], vertexArray[i2]);
}
public static Plane Convert(Microsoft.Xna.Framework.Plane plane)
{
Plane toReturn;
Convert(ref plane.Normal, out toReturn.Normal);
toReturn.D = plane.D;
return(toReturn);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="i0"></param>
/// <param name="i1"></param>
/// <param name="i2"></param>
/// <param name="vertexArray"></param>
public IndexedTriangle(int i0, int i1, int i2, List<Vector3> vertexArray)
{
counter = 0;
vertexIndices0 = i0;
vertexIndices1 = i1;
vertexIndices2 = i2;
convexFlags = unchecked((ushort)~0); // TODO check this
plane = new Microsoft.Xna.Framework.Plane(vertexArray[i0], vertexArray[i1], vertexArray[i2]);
}
public Plane(Vector3 normal, float d, ISurface surface)
{
if (surface == null)
{
throw new ArgumentNullException(nameof(surface));
}
Surface = surface;
_plane = new XPlane(normal, d);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="i0"></param>
/// <param name="i1"></param>
/// <param name="i2"></param>
/// <param name="vertexArray"></param>
public IndexedTriangle(int i0, int i1, int i2, List <Vector3> vertexArray)
{
counter = 0;
vertexIndices0 = i0;
vertexIndices1 = i1;
vertexIndices2 = i2;
convexFlags = unchecked ((ushort)~0); // TODO check this
plane = new Microsoft.Xna.Framework.Plane(vertexArray[i0], vertexArray[i1], vertexArray[i2]);
}
public static void AddPlane(this DynamicPrimitive dynamicPrimitive, Microsoft.Xna.Framework.Plane plane, float size, int tessellation, Matrix?world, Color color, float lineWidth)
{
Matrix transform = MatrixHelper.CreateRotation(Vector3.Up, plane.Normal) *
Matrix.CreateTranslation(plane.Normal * plane.D);
if (world.HasValue)
{
transform *= world.Value;
}
AddGrid(dynamicPrimitive, 0, 0, 0, size, size, tessellation, tessellation, transform, color, lineWidth);
}
public void DrawReflection(GameTime gameTime, RenderReflectionHandler render)
{
Microsoft.Xna.Framework.Plane reflectionPlane = Utilitys.CreatePlane(WaterHeight - 0.5f, Vector3.Down, ReflectionViewMatrix, Camera.Projection, true);
// Setup the Graphics Device
GraphicsDevice.SetRenderTarget(rtReflection);
// Clear the back buffer
GraphicsDevice.Clear(ClearOptions.Target | ClearOptions.DepthBuffer, Color.Black, 1.0f, 0);
// Draw anything that should reflect onto the water
render.Invoke(ReflectionViewMatrix, Camera.Position, gameTime);
// Remove our render target from our device
GraphicsDevice.SetRenderTarget(null);
// Snag our Texture from our RenderTarget
reflectionMap = rtReflection;
// For diag just save it to disk
//reflectionMap.Save(AppDomain.CurrentDomain.BaseDirectory + @"\reflectionMap.jpg", ImageFileFormat.Jpg);
}
public void DrawRefraction(GameTime gameTime, RenderRefractionHandler render)
{
// Create our Refraction Plane
Microsoft.Xna.Framework.Plane refractionPlane = Utilitys.CreatePlane(WaterHeight + 20.0f, Vector3.Down, Camera.View, Camera.Projection, false);
// Setup the graphics Device
GraphicsDevice.SetRenderTarget(rtRefraction); // <- render to our RT
// Clear the back buffer
GraphicsDevice.Clear(ClearOptions.Target | ClearOptions.DepthBuffer, Color.Black, 1.0f, 0);
render.Invoke(gameTime);
// Remove our render target from our device
GraphicsDevice.SetRenderTarget(null);
// Snag our Texture from our RenderTarget
refractionMap = rtRefraction;
// For diag just save it to disk
//refractionMap.Save(AppDomain.CurrentDomain.BaseDirectory + @"\refractionMap.jpg", ImageFileFormat.Jpg);
}
public void initPlane(GeometryHandler.Plane plane)
{
// Plane
this.planeStart = new XNA.Vector3((float)plane.Start.X, (float)plane.Start.Y, (float)plane.Start.Z);
this.planeEnd1 = new XNA.Vector3((float)plane.End1.X, (float)plane.End1.Y, (float)plane.End1.Z);
this.planeEnd2 = new XNA.Vector3((float)plane.End2.X, (float)plane.End2.Y, (float)plane.End2.Z);
this.planeDir1 = new XNA.Vector3((float)plane.Direction1.X, (float)plane.Direction1.Y, (float)plane.Direction1.Z);
this.planeDir2 = new XNA.Vector3((float)plane.Direction2.X, (float)plane.Direction2.Y, (float)plane.Direction2.Z);
this.plane = new XNA.Plane(planeStart, planeEnd1, planeEnd2);
this.planeNormal = this.plane.Normal;
}
public CollisionPackage collides(Vector3 moverRadius, Vector3 basePoint, Vector3 velocity, ICollidable collider)
{
float closestCollisionTime = 1;
Vector3 closestCollisionPoint = Vector3.Zero;
//make colliderBoundingBox a bounding volume over the movement
BoundingBox colliderBoundingBox = collider.getBoundingBox();
Vector3 worldVel = toWorldSpace(moverRadius, velocity);
if (worldVel.X < 0) colliderBoundingBox.Min.X += worldVel.X;
else if (worldVel.X > 0) colliderBoundingBox.Max.X += worldVel.X;
if (worldVel.Y < 0) colliderBoundingBox.Min.Y += worldVel.Y;
else if (worldVel.Y > 0) colliderBoundingBox.Max.Y += worldVel.Y;
if (worldVel.Z < 0) colliderBoundingBox.Min.Z += worldVel.Z;
else if (worldVel.Z > 0) colliderBoundingBox.Max.Z += worldVel.Z;
List<PickUp> pickedUp = new List<PickUp>();
List<Vector3> eSpaceVerts = new List<Vector3>();
bool convertedVerts = false;
foreach(ICollidable collidable in getCollidablesToCheck(collider, toWorldSpace(moverRadius, velocity))) {
if (collidable is Agent) continue;
BoundingBox collidableBoundingBox = collidable.getBoundingBox();
collidableBoundingBox.Min -= boundingBoxPadding;
collidableBoundingBox.Max += boundingBoxPadding;
if (!collidableBoundingBox.Intersects(colliderBoundingBox) || collidable == collider) continue;
if(collidable is Brush) {
Brush brush = (Brush)collidable;
bool faceCollide = false;
foreach (Face face in brush.faces) {
if (faceCollide)
break;
convertedVerts = false;
//find the normal of the face's plane in eSpace
Microsoft.Xna.Framework.Plane helperPlane = new Microsoft.Xna.Framework.Plane(toESpace(moverRadius, face.plane.first), toESpace(moverRadius, face.plane.second), toESpace(moverRadius, face.plane.third));
helperPlane.Normal = -helperPlane.Normal;
Vector3 N = helperPlane.Normal;
N.Normalize();
float D = -helperPlane.D;
if (Vector3.Dot(N, Vector3.Normalize(velocity)) >= -0.005)
continue;
float t0, t1;
bool embeddedInPlane = false;
if (Vector3.Dot(N, velocity) != 0) {
t0 = (-1 - signedDistance(N, basePoint, D)) / Vector3.Dot(N, velocity);
t1 = (1 - signedDistance(N, basePoint, D)) / Vector3.Dot(N, velocity);
//swap them so that t0 is smallest
if (t0 > t1) {
float temp = t1;
t1 = t0;
t0 = temp;
}
if (t0 < 0 && t1 > 1)
face.DiffuseColor = new Vector3(0, 1, 0);
if (t0 > 1 || t1 < 0)
continue;
}
else if (Math.Abs(signedDistance(N, basePoint, D)) < 1) {
t0 = 0;
t1 = 1;
embeddedInPlane = true;
}
else //in this case we can't collide with this face
continue;
if (!embeddedInPlane) {
//now we find the plane intersection point
//Vector3 planeIntersectionPoint = basePoint - N + t0 * velocity;
Vector3 planeIntersectionPoint = basePoint - N + t0 * velocity;
//project this onto the plane
//clamp [t0, t1] to [0,1]
if (t0 < 0) t0 = 0;
if (t1 < 0) t1 = 0;
if (t0 > 1) t0 = 1;
if (t1 > 1) t1 = 1;
//now we find if this point lies on the face
eSpaceVerts.Clear();
foreach (Vertex vert in face.vertices)
eSpaceVerts.Add(toESpace(moverRadius, vert.position));
convertedVerts = true;
if (MapEngine.pointOnFace(planeIntersectionPoint, eSpaceVerts, N)) {
//float intersectionDistance = t0 * (float)Math.Sqrt(Vector3.Dot(velocity, velocity));
if (face.DiffuseColor == new Vector3(1, 1, 1))
face.DiffuseColor = new Vector3(1, 0, 0);
if (t0 < closestCollisionTime) {
closestCollisionTime = t0;
closestCollisionPoint = planeIntersectionPoint;
faceCollide = true;
continue;
}
}
}
if (!convertedVerts) {
eSpaceVerts.Clear();
foreach (Vertex vert in face.vertices)
eSpaceVerts.Add(toESpace(moverRadius, vert.position));
convertedVerts = true;
}
//do sweep tests
//sweep against the vertices
foreach (Vector3 p in eSpaceVerts) {
//calculate A, B and C that make up our quadratic equation
// At^2 + Bt + C = 0
float A = Vector3.Dot(velocity, velocity),
B = 2 * Vector3.Dot(velocity, basePoint - p),
C = Vector3.Dot(p - basePoint, p - basePoint) - 1;
//check that the equation has a solution
float discriminant = B * B - 4 * A * C;
//in this case we have a solution to the equation
if (discriminant >= 0) {
float r1 = (-B + (float)Math.Sqrt(discriminant)) / (2 * A),
r2 = (-B - (float)Math.Sqrt(discriminant)) / (2 * A);
/*float x1 = Math.Min(r1, r2), x2 = Math.Max(r1, r2);
if (x1 < 0)
x1 = x2;*/
if (r1 > r2) {
float temp = r2;
r2 = r1;
r1 = temp;
}
float root = -1;
if (r1 > 0 && r1 < closestCollisionTime)
root = r1;
else if (r2 > 0 && r2 < closestCollisionTime)
root = r2;
/* intersectionPoint = p;
* intersectionDistance = x1 * (float)Math.Sqrt(Vector3.Dot( velocity, velocity ));
*/
if (root != -1) {
closestCollisionTime = root;
closestCollisionPoint = p;
faceCollide = true;
}
}
}
//sweep against the edges
for (int i = 0; i < eSpaceVerts.Count; i++) {
//calculate A, B and C that make up our quadratic equation
// At^2 + Bt + C = 0
Vector3 p1 = eSpaceVerts[i],
p2 = eSpaceVerts[(i + 1) % eSpaceVerts.Count];
Vector3 edge = p2 - p1;
Vector3 baseToVertex = p1 - basePoint;
float A = Vector3.Dot(edge, edge) * (-Vector3.Dot(velocity, velocity)) + (float)Math.Pow(Vector3.Dot(edge, velocity), 2),
B = Vector3.Dot(edge, edge) * 2 * Vector3.Dot(velocity, baseToVertex) - 2 * (Vector3.Dot(edge, velocity) * Vector3.Dot(edge, baseToVertex)),
C = Vector3.Dot(edge, edge) * (1 - Vector3.Dot(baseToVertex, baseToVertex)) + (float)Math.Pow(Vector3.Dot(edge, baseToVertex), 2);
//check that the equation has a solution
float discriminant = B * B - 4 * A * C;
//in this case we have a solution to the equation
if (discriminant >= 0) {
float r1 = (-B + (float)Math.Sqrt(discriminant)) / (2 * A),
r2 = (-B - (float)Math.Sqrt(discriminant)) / (2 * A);
if (r1 > r2) {
float temp = r2;
r2 = r1;
r1 = temp;
}
float root = -1;
if (r1 > 0 && r1 < closestCollisionTime)
root = r1;
else if (r2 > 0 && r2 < closestCollisionTime)
root = r2;
if (root != -1) {
float f0 = (Vector3.Dot(edge, velocity) * root - Vector3.Dot(edge, baseToVertex)) / (Vector3.Dot(edge, edge));
//NEED TO DO SOMETHING HERE--------------------------------------------------------------------------
/* intersectionPoint = p1 + f0 * edge;
* intersectionDistance = x1 * (float)Math.Sqrt(Vector3.Dot( velocity, velocity ));
*/
if (0 <= f0 && f0 <= 1) {
closestCollisionTime = root;
closestCollisionPoint = p1 + f0 * edge;
faceCollide = true;
}
}
}
}
}
} // if brush
else if(collider is Agent && collidable is PickUp) { //otherwise we need to boundingEllipsoid collisions
PickUp p = (PickUp)collidable;
if (collidesWithPickup(moverRadius, basePoint, velocity, p))
pickedUp.Add(p);
} // if pickup
}
foreach (PickUp p in pickedUp) {
p.pickupGen.removePickUp();
p.affect((Agent)collider);
}
return new CollisionPackage(basePoint, velocity, closestCollisionTime < 1, closestCollisionPoint, closestCollisionTime * (float)Math.Sqrt(Vector3.Dot(velocity, velocity)));
}
public bool Select(Ray ray)
{
Vector3 crossVector = Vector3.Cross(FirstVertex - SecondVertex, ray.Position - SecondVertex);
Microsoft.Xna.Framework.Plane plane = new Microsoft.Xna.Framework.Plane(FirstVertex, SecondVertex, crossVector + SecondVertex);
float? dist = ray.Intersects(plane);
if (!dist.HasValue)
return false;
Vector3 intersectPoint = ray.Position + ray.Direction * dist.Value;
if (Vector3.Distance(intersectPoint, FirstVertex) > Length)
return false;
if (Vector3.Distance(intersectPoint, SecondVertex) > Length)
return false;
float intersectDist = Vector3.Cross(SecondVertex - FirstVertex, intersectPoint - FirstVertex).Length() / Vector3.Distance(SecondVertex, FirstVertex);
return intersectDist < 0.1f;
}
public static void Convert(ref Microsoft.Xna.Framework.Plane plane, out Plane bepuPlane)
{
Convert(ref plane.Normal, out bepuPlane.Normal);
bepuPlane.D = plane.D;
}
public static void Convert(ref Plane plane, out Microsoft.Xna.Framework.Plane xnaPlane)
{
Convert(ref plane.Normal, out xnaPlane.Normal);
xnaPlane.D = plane.D;
}
public static bool SweptSphereTriangleIntersection(out Vector3 pt, out Vector3 N, out float depth,
BoundingSphere oldSphere, BoundingSphere newSphere, Triangle triangle,
float oldCentreDistToPlane, float newCentreDistToPlane,
EdgesToTest edgesToTest, CornersToTest cornersToTest)
{
int i;
Microsoft.Xna.Framework.Plane trianglePlane = triangle.Plane;
N = Vector3.Zero;
// Check against plane
if (!SweptSpherePlaneIntersection(out pt, out depth, oldSphere, newSphere, trianglePlane.Normal, oldCentreDistToPlane, newCentreDistToPlane))
{
return(false);
}
Vector3 v0 = triangle.GetPoint(0);
Vector3 v1 = triangle.GetPoint(1);
Vector3 v2 = triangle.GetPoint(2);
Vector3 e0 = v1 - v0;
Vector3 e1 = v2 - v1;
Vector3 e2 = v0 - v2;
// If the point is inside the triangle, this is a hit
bool allInside = true;
Vector3 outDir0 = Vector3.Cross(e0, trianglePlane.Normal);
if (Vector3.Dot(pt - v0, outDir0) > 0.0f)
{
allInside = false;
}
Vector3 outDir1 = Vector3.Cross(e1, trianglePlane.Normal);
if (Vector3.Dot(pt - v1, outDir1) > 0.0f)
{
allInside = false;
}
Vector3 outDir2 = Vector3.Cross(e2, trianglePlane.Normal);
if (Vector3.Dot(pt - v2, outDir2) > 0.0f)
{
allInside = false;
}
// Quick result?
if (allInside)
{
N = trianglePlane.Normal;
return(true);
}
// Now check against the edges
float bestT = float.MaxValue;
Vector3 Ks = newSphere.Center - oldSphere.Center;
float kss = Vector3.Dot(Ks, Ks);
float radius = newSphere.Radius;
float radiusSq = radius * radius;
for (i = 0; i < 3; ++i)
{
int mask = 1 << i;
if (!((mask != 0) & ((int)edgesToTest != 0))) // TODO: CHECK THIS
{
continue;
}
Vector3 Ke;
Vector3 vp;
switch (i)
{
case 0:
Ke = e0;
vp = v0;
break;
case 1:
Ke = e1;
vp = v1;
break;
case 2:
default:
Ke = e2;
vp = v2;
break;
}
Vector3 Kg = vp - oldSphere.Center;
float kee = Vector3.Dot(Ke, Ke);
if (System.Math.Abs(kee) < JiggleMath.Epsilon)
{
continue;
}
float kes = Vector3.Dot(Ke, Ks);
float kgs = Vector3.Dot(Kg, Ks);
float keg = Vector3.Dot(Ke, Kg);
float kgg = Vector3.Dot(Kg, Kg);
// a * t^2 + b * t + c = 0
float a = kee * kss - (kes * kes);
if (System.Math.Abs(a) < JiggleMath.Epsilon)
{
continue;
}
float b = 2.0f * (keg * kes - kee * kgs);
float c = kee * (kgg - radiusSq) - keg * keg;
float blah = b * b - 4.0f * a * c;
if (blah < 0.0f)
{
continue;
}
// solve for t - take minimum
float t = (-b - (float)System.Math.Sqrt(blah)) / (2.0f * a);
if (t < 0.0f || t > 1.0f)
{
continue;
}
if (t > bestT)
{
continue;
}
// now check where it hit on the edge
Vector3 Ct = oldSphere.Center + t * Ks;
float d = Vector3.Dot((Ct - vp), Ke) / kee;
if (d < 0.0f || d > 1.0f)
{
continue;
}
// wahay - got hit. Already checked that t < bestT
bestT = t;
pt = vp + d * Ke;
N = (Ct - pt);// .GetNormalisedSafe();
JiggleMath.NormalizeSafe(ref N);
// depth is already calculated
}
if (bestT <= 1.0f)
{
return(true);
}
// check the corners
bestT = float.MaxValue;
for (i = 0; i < 3; ++i)
{
int mask = 1 << i;
if (!((mask != 0) & (cornersToTest != 0))) // CHECK THIS
{
continue;
}
Vector3 vp;
switch (i)
{
case 0:
vp = v0;
break;
case 1:
vp = v1;
break;
case 2:
default:
vp = v2;
break;
}
Vector3 Kg = vp - oldSphere.Center;
float kgs = Vector3.Dot(Kg, Ks);
float kgg = Vector3.Dot(Kg, Kg);
float a = kss;
if (System.Math.Abs(a) < JiggleMath.Epsilon)
{
continue;
}
float b = -2.0f * kgs;
float c = kgg - radiusSq;
float blah = (b * b) - 4.0f * a * c;
if (blah < 0.0f)
{
continue;
}
// solve for t - take minimum
float t = (-b - (float)System.Math.Sqrt(blah)) / (2.0f * a);
if (t < 0.0f || t > 1.0f)
{
continue;
}
if (t > bestT)
{
continue;
}
bestT = t;
Vector3 Ct = oldSphere.Center + t * Ks;
N = (Ct - vp);//.GetNormalisedSafe();
JiggleMath.NormalizeSafe(ref N);
}
if (bestT <= 1.0f)
{
return(true);
}
return(false);
}
public CollisionPackage collides(Vector3 moverRadius, Vector3 basePoint, Vector3 velocity, ICollidable collider)
{
float closestCollisionTime = 1;
Vector3 closestCollisionPoint = Vector3.Zero;
//make colliderBoundingBox a bounding volume over the movement
BoundingBox colliderBoundingBox = collider.getBoundingBox();
Vector3 worldVel = toWorldSpace(moverRadius, velocity);
if (worldVel.X < 0)
{
colliderBoundingBox.Min.X += worldVel.X;
}
else if (worldVel.X > 0)
{
colliderBoundingBox.Max.X += worldVel.X;
}
if (worldVel.Y < 0)
{
colliderBoundingBox.Min.Y += worldVel.Y;
}
else if (worldVel.Y > 0)
{
colliderBoundingBox.Max.Y += worldVel.Y;
}
if (worldVel.Z < 0)
{
colliderBoundingBox.Min.Z += worldVel.Z;
}
else if (worldVel.Z > 0)
{
colliderBoundingBox.Max.Z += worldVel.Z;
}
List <PickUp> pickedUp = new List <PickUp>();
List <Vector3> eSpaceVerts = new List <Vector3>();
bool convertedVerts = false;
foreach (ICollidable collidable in getCollidablesToCheck(collider, toWorldSpace(moverRadius, velocity)))
{
if (collidable is Agent)
{
continue;
}
BoundingBox collidableBoundingBox = collidable.getBoundingBox();
collidableBoundingBox.Min -= boundingBoxPadding;
collidableBoundingBox.Max += boundingBoxPadding;
if (!collidableBoundingBox.Intersects(colliderBoundingBox) || collidable == collider)
{
continue;
}
if (collidable is Brush)
{
Brush brush = (Brush)collidable;
bool faceCollide = false;
foreach (Face face in brush.faces)
{
if (faceCollide)
{
break;
}
convertedVerts = false;
//find the normal of the face's plane in eSpace
Microsoft.Xna.Framework.Plane helperPlane = new Microsoft.Xna.Framework.Plane(toESpace(moverRadius, face.plane.first), toESpace(moverRadius, face.plane.second), toESpace(moverRadius, face.plane.third));
helperPlane.Normal = -helperPlane.Normal;
Vector3 N = helperPlane.Normal;
N.Normalize();
float D = -helperPlane.D;
if (Vector3.Dot(N, Vector3.Normalize(velocity)) >= -0.005)
{
continue;
}
float t0, t1;
bool embeddedInPlane = false;
if (Vector3.Dot(N, velocity) != 0)
{
t0 = (-1 - signedDistance(N, basePoint, D)) / Vector3.Dot(N, velocity);
t1 = (1 - signedDistance(N, basePoint, D)) / Vector3.Dot(N, velocity);
//swap them so that t0 is smallest
if (t0 > t1)
{
float temp = t1;
t1 = t0;
t0 = temp;
}
if (t0 < 0 && t1 > 1)
{
face.DiffuseColor = new Vector3(0, 1, 0);
}
if (t0 > 1 || t1 < 0)
{
continue;
}
}
else if (Math.Abs(signedDistance(N, basePoint, D)) < 1)
{
t0 = 0;
t1 = 1;
embeddedInPlane = true;
}
else //in this case we can't collide with this face
{
continue;
}
if (!embeddedInPlane)
{
//now we find the plane intersection point
//Vector3 planeIntersectionPoint = basePoint - N + t0 * velocity;
Vector3 planeIntersectionPoint = basePoint - N + t0 * velocity;
//project this onto the plane
//clamp [t0, t1] to [0,1]
if (t0 < 0)
{
t0 = 0;
}
if (t1 < 0)
{
t1 = 0;
}
if (t0 > 1)
{
t0 = 1;
}
if (t1 > 1)
{
t1 = 1;
}
//now we find if this point lies on the face
eSpaceVerts.Clear();
foreach (Vertex vert in face.vertices)
{
eSpaceVerts.Add(toESpace(moverRadius, vert.position));
}
convertedVerts = true;
if (MapEngine.pointOnFace(planeIntersectionPoint, eSpaceVerts, N))
{
//float intersectionDistance = t0 * (float)Math.Sqrt(Vector3.Dot(velocity, velocity));
if (face.DiffuseColor == new Vector3(1, 1, 1))
{
face.DiffuseColor = new Vector3(1, 0, 0);
}
if (t0 < closestCollisionTime)
{
closestCollisionTime = t0;
closestCollisionPoint = planeIntersectionPoint;
faceCollide = true;
continue;
}
}
}
if (!convertedVerts)
{
eSpaceVerts.Clear();
foreach (Vertex vert in face.vertices)
{
eSpaceVerts.Add(toESpace(moverRadius, vert.position));
}
convertedVerts = true;
}
//do sweep tests
//sweep against the vertices
foreach (Vector3 p in eSpaceVerts)
{
//calculate A, B and C that make up our quadratic equation
// At^2 + Bt + C = 0
float A = Vector3.Dot(velocity, velocity),
B = 2 * Vector3.Dot(velocity, basePoint - p),
C = Vector3.Dot(p - basePoint, p - basePoint) - 1;
//check that the equation has a solution
float discriminant = B * B - 4 * A * C;
//in this case we have a solution to the equation
if (discriminant >= 0)
{
float r1 = (-B + (float)Math.Sqrt(discriminant)) / (2 * A),
r2 = (-B - (float)Math.Sqrt(discriminant)) / (2 * A);
/*float x1 = Math.Min(r1, r2), x2 = Math.Max(r1, r2);
* if (x1 < 0)
* x1 = x2;*/
if (r1 > r2)
{
float temp = r2;
r2 = r1;
r1 = temp;
}
float root = -1;
if (r1 > 0 && r1 < closestCollisionTime)
{
root = r1;
}
else if (r2 > 0 && r2 < closestCollisionTime)
{
root = r2;
}
/* intersectionPoint = p;
* intersectionDistance = x1 * (float)Math.Sqrt(Vector3.Dot( velocity, velocity ));
*/
if (root != -1)
{
closestCollisionTime = root;
closestCollisionPoint = p;
faceCollide = true;
}
}
}
//sweep against the edges
for (int i = 0; i < eSpaceVerts.Count; i++)
{
//calculate A, B and C that make up our quadratic equation
// At^2 + Bt + C = 0
Vector3 p1 = eSpaceVerts[i],
p2 = eSpaceVerts[(i + 1) % eSpaceVerts.Count];
Vector3 edge = p2 - p1;
Vector3 baseToVertex = p1 - basePoint;
float A = Vector3.Dot(edge, edge) * (-Vector3.Dot(velocity, velocity)) + (float)Math.Pow(Vector3.Dot(edge, velocity), 2),
B = Vector3.Dot(edge, edge) * 2 * Vector3.Dot(velocity, baseToVertex) - 2 * (Vector3.Dot(edge, velocity) * Vector3.Dot(edge, baseToVertex)),
C = Vector3.Dot(edge, edge) * (1 - Vector3.Dot(baseToVertex, baseToVertex)) + (float)Math.Pow(Vector3.Dot(edge, baseToVertex), 2);
//check that the equation has a solution
float discriminant = B * B - 4 * A * C;
//in this case we have a solution to the equation
if (discriminant >= 0)
{
float r1 = (-B + (float)Math.Sqrt(discriminant)) / (2 * A),
r2 = (-B - (float)Math.Sqrt(discriminant)) / (2 * A);
if (r1 > r2)
{
float temp = r2;
r2 = r1;
r1 = temp;
}
float root = -1;
if (r1 > 0 && r1 < closestCollisionTime)
{
root = r1;
}
else if (r2 > 0 && r2 < closestCollisionTime)
{
root = r2;
}
if (root != -1)
{
float f0 = (Vector3.Dot(edge, velocity) * root - Vector3.Dot(edge, baseToVertex)) / (Vector3.Dot(edge, edge));
//NEED TO DO SOMETHING HERE--------------------------------------------------------------------------
/* intersectionPoint = p1 + f0 * edge;
* intersectionDistance = x1 * (float)Math.Sqrt(Vector3.Dot( velocity, velocity ));
*/
if (0 <= f0 && f0 <= 1)
{
closestCollisionTime = root;
closestCollisionPoint = p1 + f0 * edge;
faceCollide = true;
}
}
}
}
}
} // if brush
else if (collider is Agent && collidable is PickUp) //otherwise we need to boundingEllipsoid collisions
{
PickUp p = (PickUp)collidable;
if (collidesWithPickup(moverRadius, basePoint, velocity, p))
{
pickedUp.Add(p);
}
} // if pickup
}
foreach (PickUp p in pickedUp)
{
p.pickupGen.removePickUp();
p.affect((Agent)collider);
}
return(new CollisionPackage(basePoint, velocity, closestCollisionTime < 1, closestCollisionPoint, closestCollisionTime * (float)Math.Sqrt(Vector3.Dot(velocity, velocity))));
}
/// <summary>
/// Set the indices into the relevant vertex array for this
/// triangle. Also sets the plane and bounding box
/// </summary>
/// <param name="i0"></param>
/// <param name="i1"></param>
/// <param name="i2"></param>
/// <param name="vertexArray"></param>
public void SetVertexIndices(int i0, int i1, int i2, Vector3[] vertexArray)
{
vertexIndices0 = i0;
vertexIndices1 = i1;
vertexIndices2 = i2;
plane = new Microsoft.Xna.Framework.Plane(vertexArray[i0], vertexArray[i1], vertexArray[i2]);
}
public Plane(Vector3 normal, float d, ISurface surface)
{
Surface = surface;
_plane = new XPlane(normal, d);
}
public Plane(XNAPlane plane, Material material)
{
this.plane = plane;
this.Material = material;
}
// Returns the intersection of given vector and given plane
public Point3D intersectVectorPlane(Plane p, Vector v)
{
XNA.Vector3 vec0 = new XNA.Vector3((float)v.Start.X, (float)v.Start.Y, (float)v.Start.Z);
XNA.Vector3 vec1 = new XNA.Vector3((float)p.Start.X, (float)p.Start.Y, (float)p.Start.Z);
XNA.Vector3 vec2 = new XNA.Vector3((float)p.End1.X, (float)p.End1.Y, (float)p.End1.Z);
XNA.Vector3 vec3 = new XNA.Vector3((float)p.End2.X, (float)p.End2.Y, (float)p.End2.Z);
XNA.Vector3 vec5 = new XNA.Vector3((float)v.Direction.X, (float)v.Direction.Y, (float)v.Direction.Z);
XNA.Ray ray = new XNA.Ray(vec0, vec5);
XNA.Plane plane = new XNA.Plane(vec1, vec2, vec3);
double s = (double)XNA.Vector3.Dot(plane.Normal, (vec1 - vec0)) / XNA.Vector3.Dot(plane.Normal, vec5);
Point3D intersection = v.Start + (s * v.Direction);
return intersection;
}
