//FIN OPTIMIZACION
//----------------------------------------
public bool intersectRayAABB(TgcRay.RayStruct ray, TgcBoundingAxisAlignBox aabb, out float tmin,
out Vector3 q)
//, out float tmin, out Vector3 q)
{
var aabbMin = TgcCollisionUtils.toArray(aabb.PMin);
var aabbMax = TgcCollisionUtils.toArray(aabb.PMax);
var p = TgcCollisionUtils.toArray(ray.origin);
var d = TgcCollisionUtils.toArray(ray.direction);
tmin = 0.0f; // set to -FLT_MAX to get first hit on line
var tmax = float.MaxValue; // set to max distance ray can travel (for segment)
q = Vector3.Empty;
// For all three slabs
for (var i = 0; i < 3; i++)
{
if (FastMath.Abs(d[i]) < float.Epsilon)
{
// Ray is parallel to slab. No hit if origin not within slab
if (p[i] < aabbMin[i] || p[i] > aabbMax[i])
{
return(false);
}
}
else
{
// Compute intersection t value of ray with near and far plane of slab
var ood = 1.0f / d[i];
var t1 = (aabbMin[i] - p[i]) * ood;
var t2 = (aabbMax[i] - p[i]) * ood;
// Make t1 be intersection with near plane, t2 with far plane
if (t1 > t2)
{
TgcCollisionUtils.swap(ref t1, ref t2);
}
// Compute the intersection of slab intersection intervals
tmin = TgcCollisionUtils.max(tmin, t1);
tmax = TgcCollisionUtils.min(tmax, t2);
// Exit with no collision as soon as slab intersection becomes empty
if (tmin > tmax)
{
return(false);
}
}
}
// Ray intersects all 3 slabs. Return point (q) and intersection t value (tmin)
q = ray.origin + ray.direction * tmin;
return(true);
}
/// <summary>
/// Detectar colision entre un Segmento de recta y una Capsula.
/// </summary>
/// <param name="seg">Segmento</param>
/// <param name="capsule">Capsula</param>
/// <param name="t">Menor instante de colision</param>
/// <returns>True si hay colision</returns>
private bool intersectSegmentCapsule(Segment seg, Capsule capsule, out float t)
{
TgcRay.RayStruct ray = new TgcRay.RayStruct();
ray.origin = seg.a;
ray.direction = seg.dir;
if (intersectRayCapsule(ray, capsule, out t))
{
if (t >= 0.0f && t <= seg.length)
{
t /= seg.length;
return(true);
}
}
return(false);
}
/// <summary>
/// Colisiona un BoundingSphere en movimiento contra el BoundingBox.
/// Si hay colision devuelve el instante t de colision mas proximo y el punto q de colision mas cercano
/// </summary>
/// <param name="sphere">BoundingSphere</param>
/// <param name="movementVector">movimiento del BoundingSphere</param>
/// <param name="t">Menor instante de colision</param>
/// <param name="q">Punto mas cercano de colision</param>
/// <param name="n">Normal de la cara colisionada</param>
/// <returns>True si hay colision</returns>
public override bool intersectMovingSphere(TgcBoundingSphere sphere, TGCVector3 movementVector,
TgcBoundingSphere movementSphere, out float t, out TGCVector3 q, out TGCVector3 n)
{
t = -1f;
q = TGCVector3.Empty;
n = TGCVector3.Empty;
// Compute the AABB resulting from expanding b by sphere radius r
var e = Aabb.toStruct();
e.min.X -= sphere.Radius;
e.min.Y -= sphere.Radius;
e.min.Z -= sphere.Radius;
e.max.X += sphere.Radius;
e.max.Y += sphere.Radius;
e.max.Z += sphere.Radius;
// Intersect ray against expanded AABB e. Exit with no intersection if ray
// misses e, else get intersection point p and time t as result
TGCVector3 p;
var ray = new TgcRay.RayStruct();
ray.origin = sphere.Center;
ray.direction = movementVector;
if (!intersectRayAABB(ray, e, out t, out p) || t > 1.0f)
{
return(false);
}
// Compute which min and max faces of b the intersection point p lies
// outside of. Note, u and v cannot have the same bits set and
// they must have at least one bit set among them
var i = 0;
var sign = new int[3];
if (p.X < Aabb.PMin.X)
{
sign[0] = -1;
i++;
}
if (p.X > Aabb.PMax.X)
{
sign[0] = 1;
i++;
}
if (p.Y < Aabb.PMin.Y)
{
sign[1] = -1;
i++;
}
if (p.Y > Aabb.PMax.Y)
{
sign[1] = 1;
i++;
}
if (p.Z < Aabb.PMin.Z)
{
sign[2] = -1;
i++;
}
if (p.Z > Aabb.PMax.Z)
{
sign[2] = 1;
i++;
}
//Face
if (i == 1)
{
n = new TGCVector3(sign[0], sign[1], sign[2]);
q = sphere.Center + t * movementVector - sphere.Radius * n;
return(true);
}
// Define line segment [c, c+d] specified by the sphere movement
var seg = new Segment(sphere.Center, sphere.Center + movementVector);
//Box extent and center
var extent = Aabb.calculateAxisRadius();
var center = Aabb.PMin + extent;
//Edge
if (i == 2)
{
//Generar los dos puntos extremos del Edge
float[] extentDir = { sign[0], sign[1], sign[2] };
var zeroIndex = sign[0] == 0 ? 0 : (sign[1] == 0 ? 1 : 2);
extentDir[zeroIndex] = 1;
var capsuleA = center + new TGCVector3(extent.X * extentDir[0], extent.Y * extentDir[1], extent.Z * extentDir[2]);
extentDir[zeroIndex] = -1;
var capsuleB = center + new TGCVector3(extent.X * extentDir[0], extent.Y * extentDir[1], extent.Z * extentDir[2]);
//Colision contra el Edge hecho Capsula
if (intersectSegmentCapsule(seg, new Capsule(capsuleA, capsuleB, sphere.Radius), out t))
{
n = new TGCVector3(sign[0], sign[1], sign[2]);
n.Normalize();
q = sphere.Center + t * movementVector - sphere.Radius * n;
return(true);
}
}
//Vertex
if (i == 3)
{
var tmin = float.MaxValue;
var capsuleA = center + new TGCVector3(extent.X * sign[0], extent.Y * sign[1], extent.Z * sign[2]);
TGCVector3 capsuleB;
capsuleB = center + new TGCVector3(extent.X * -sign[0], extent.Y * sign[1], extent.Z * sign[2]);
if (intersectSegmentCapsule(seg, new Capsule(capsuleA, capsuleB, sphere.Radius), out t))
{
tmin = TgcCollisionUtils.min(t, tmin);
}
capsuleB = center + new TGCVector3(extent.X * sign[0], extent.Y * -sign[1], extent.Z * sign[2]);
if (intersectSegmentCapsule(seg, new Capsule(capsuleA, capsuleB, sphere.Radius), out t))
{
tmin = TgcCollisionUtils.min(t, tmin);
}
capsuleB = center + new TGCVector3(extent.X * sign[0], extent.Y * sign[1], extent.Z * -sign[2]);
if (intersectSegmentCapsule(seg, new Capsule(capsuleA, capsuleB, sphere.Radius), out t))
{
tmin = TgcCollisionUtils.min(t, tmin);
}
if (tmin == float.MaxValue)
{
return(false); // No intersection
}
t = tmin;
n = new TGCVector3(sign[0], sign[1], sign[2]);
n.Normalize();
q = sphere.Center + t * movementVector - sphere.Radius * n;
return(true); // Intersection at time t == tmin
}
return(false);
}
/// <summary>
/// Detectar colision entre un Ray y una Capsula
/// Basado en: http://www.geometrictools.com/LibMathematics/Intersection/Wm5IntrLine3Capsule3.cpp
/// </summary>
/// <param name="ray">Ray</param>
/// <param name="capsule">Capsula</param>
/// <param name="t">Menor instante de colision</param>
/// <returns>True si hay colision</returns>
private bool intersectRayCapsule(TgcRay.RayStruct ray, Capsule capsule, out float t)
{
t = -1;
var origin = ray.origin;
var dir = ray.direction;
// Create a coordinate system for the capsule. In this system, the
// capsule segment center C is the origin and the capsule axis direction
// W is the z-axis. U and V are the other coordinate axis directions.
// If P = x*U+y*V+z*W, the cylinder containing the capsule plane is
// x^2 + y^2 = r^2, where r is the capsule radius. The finite cylinder
// that makes up the capsule minus its hemispherical end caps has z-values
// |z| <= e, where e is the extent of the capsule segment. The top
// hemisphere cap is x^2+y^2+(z-e)^2 = r^2 for z >= e, and the bottom
// hemisphere cap is x^2+y^2+(z+e)^2 = r^2 for z <= -e.
var U = capsule.segment.dir;
var V = U;
var W = U;
generateComplementBasis(ref U, ref V, W);
var rSqr = capsule.radius * capsule.radius;
var extent = capsule.segment.extent;
// Convert incoming line origin to capsule coordinates.
var diff = origin - capsule.segment.center;
var P = new TGCVector3(TGCVector3.Dot(U, diff), TGCVector3.Dot(V, diff), TGCVector3.Dot(W, diff));
// Get the z-value, in capsule coordinates, of the incoming line's unit-length direction.
var dz = TGCVector3.Dot(W, dir);
if (FastMath.Abs(dz) >= 1f - float.Epsilon)
{
// The line is parallel to the capsule axis. Determine whether the line intersects the capsule hemispheres.
var radialSqrDist = rSqr - P.X * P.X - P.Y * P.Y;
if (radialSqrDist < 0f)
{
// Line outside the cylinder of the capsule, no intersection.
return(false);
}
// line intersects the hemispherical caps
var zOffset = FastMath.Sqrt(radialSqrDist) + extent;
if (dz > 0f)
{
t = -P.Z - zOffset;
}
else
{
t = P.Z - zOffset;
}
return(true);
}
// Convert incoming line unit-length direction to capsule coordinates.
var D = new TGCVector3(TGCVector3.Dot(U, dir), TGCVector3.Dot(V, dir), dz);
// Test intersection of line P+t*D with infinite cylinder x^2+y^2 = r^2.
// This reduces to computing the roots of a quadratic equation. If
// P = (px,py,pz) and D = (dx,dy,dz), then the quadratic equation is
// (dx^2+dy^2)*t^2 + 2*(px*dx+py*dy)*t + (px^2+py^2-r^2) = 0
var a0 = P.X * P.X + P.Y * P.Y - rSqr;
var a1 = P.X * D.X + P.Y * D.Y;
var a2 = D.X * D.X + D.Y * D.Y;
var discr = a1 * a1 - a0 * a2;
if (discr < 0f)
{
// Line does not intersect infinite cylinder.
return(false);
}
float root, inv, tValue, zValue;
var quantity = 0;
if (discr > float.Epsilon)
{
// Line intersects infinite cylinder in two places.
root = FastMath.Sqrt(discr);
inv = 1f / a2;
tValue = (-a1 - root) * inv;
zValue = P.Z + tValue * D.Z;
if (FastMath.Abs(zValue) <= extent)
{
quantity++;
t = tValue;
}
tValue = (-a1 + root) * inv;
zValue = P.Z + tValue * D.Z;
if (FastMath.Abs(zValue) <= extent)
{
quantity++;
t = TgcCollisionUtils.min(t, tValue);
}
if (quantity == 2)
{
// Line intersects capsule plane in two places.
return(true);
}
}
else
{
// Line is tangent to infinite cylinder.
tValue = -a1 / a2;
zValue = P.Z + tValue * D.Z;
if (FastMath.Abs(zValue) <= extent)
{
t = tValue;
return(true);
}
}
// Test intersection with bottom hemisphere. The quadratic equation is
// t^2 + 2*(px*dx+py*dy+(pz+e)*dz)*t + (px^2+py^2+(pz+e)^2-r^2) = 0
// Use the fact that currently a1 = px*dx+py*dy and a0 = px^2+py^2-r^2.
// The leading coefficient is a2 = 1, so no need to include in the
// construction.
var PZpE = P.Z + extent;
a1 += PZpE * D.Z;
a0 += PZpE * PZpE;
discr = a1 * a1 - a0;
if (discr > float.Epsilon)
{
root = FastMath.Sqrt(discr);
tValue = -a1 - root;
zValue = P.Z + tValue * D.Z;
if (zValue <= -extent)
{
quantity++;
t = TgcCollisionUtils.min(t, tValue);
if (quantity == 2)
{
return(true);
}
}
tValue = -a1 + root;
zValue = P.Z + tValue * D.Z;
if (zValue <= -extent)
{
quantity++;
t = TgcCollisionUtils.min(t, tValue);
if (quantity == 2)
{
return(true);
}
}
}
else if (FastMath.Abs(discr) <= float.Epsilon)
{
tValue = -a1;
zValue = P.Z + tValue * D.Z;
if (zValue <= -extent)
{
quantity++;
t = TgcCollisionUtils.min(t, tValue);
if (quantity == 2)
{
return(true);
}
}
}
// Test intersection with top hemisphere. The quadratic equation is
// t^2 + 2*(px*dx+py*dy+(pz-e)*dz)*t + (px^2+py^2+(pz-e)^2-r^2) = 0
// Use the fact that currently a1 = px*dx+py*dy+(pz+e)*dz and
// a0 = px^2+py^2+(pz+e)^2-r^2. The leading coefficient is a2 = 1, so
// no need to include in the construction.
a1 -= 2f * extent * D.Z;
a0 -= 4 * extent * P.Z;
discr = a1 * a1 - a0;
if (discr > float.Epsilon)
{
root = FastMath.Sqrt(discr);
tValue = -a1 - root;
zValue = P.Z + tValue * D.Z;
if (zValue >= extent)
{
quantity++;
t = TgcCollisionUtils.min(t, tValue);
if (quantity == 2)
{
return(true);
}
}
tValue = -a1 + root;
zValue = P.Z + tValue * D.Z;
if (zValue >= extent)
{
quantity++;
t = TgcCollisionUtils.min(t, tValue);
if (quantity == 2)
{
return(true);
}
}
}
else if (FastMath.Abs(discr) <= float.Epsilon)
{
tValue = -a1;
zValue = P.Z + tValue * D.Z;
if (zValue >= extent)
{
quantity++;
t = TgcCollisionUtils.min(t, tValue);
if (quantity == 2)
{
return(true);
}
}
}
return(quantity > 0);
}
private void ProcesarColisiones()
{
collisionFound = false;
chocoAdelante = false;
//TgcCollisionUtils.testobbTest choque cilindro
var ray = new TgcRay.RayStruct();
var x1 = -largo *FastMath.Sin(anguloFinal);
var z1 = -largo *FastMath.Cos(anguloFinal);
var x2 = x1 * 1.2;
var z2 = z1 * 1.2;
//var a = Mesh.Position.TransformCoordinate(Matrix.Identity);
ray.origin = new Vector3(
Mesh.Position.X + x1,
Mesh.Position.Y,
Mesh.Position.Z + z1);
ray.direction = new Vector3(
newPosicion.X + (float)x2,
newPosicion.Y,
newPosicion.Z + (float)z2
);
directionArrow = new TgcArrow();
directionArrow.Thickness = 5;
directionArrow.HeadSize = new Vector2(10, 10);
//directionArrow.PEnd = ray.origin;
directionArrow.PStart = ray.origin;
directionArrow.PEnd = ray.direction;
directionArrow.updateValues();
//-FastMath.Sin(anguloFinal), 0, 350 * -FastMath.Cos(anguloFinal)
//ray.direction = eMovementVector;
foreach (var sceneMesh in ciudadScene.Meshes)
{
var escenaAABB = sceneMesh.BoundingBox;
var collisionResult = TgcCollisionUtils.testObbAABB(obb, escenaAABB);
//2 -si lo hizo, salgo del foreach.
if (collisionResult)
{
collisionFound = true;
//if (intersectRayAABB(ray, escenaAABB))//, out t, out p) || t > 1.0f)
float t;
Vector3 p;
chocoAdelante = (intersectRayAABB(ray, escenaAABB, out t, out p) || t > 1.0f);
break;
}
}
//3 - si chocó, pongo el bounding box en rojo (apretar F para ver el bb).
if (collisionFound)
{
if (Mesh.Position.Y == 5 || Mesh.Position.Y >= 25)
{
obb.setRenderColor(Color.Red);
//efectoShaderNitroHummer.SetValue("Velocidad", 4 * Velocidad);
PosicionRollback();
}
}
else
{
obb.setRenderColor(Color.Yellow);
}
if (Mesh.Position.Y == 5)
{
ManejarColisionCamara();
}
velocimetro.Update(Velocidad, marchaAtras);
}
/// <summary>
/// Colisiona un Elipsoide en movimiento contra el BoundingBox.
/// Si hay colision devuelve el instante t de colision, el punto q de colision y el vector normal n de la superficie contra la que
/// se colisiona.
/// Todo se devuelve en Elipsoid space.
/// El BoundingBox se pasa a Elipsoid space para comparar.
/// </summary>
/// <param name="eSphere">BoundingSphere de radio 1 en Elipsoid space</param>
/// <param name="eMovementVector">movimiento en Elipsoid space</param>
/// <param name="eRadius">radio del Elipsoide</param>
/// <param name="movementSphere">BoundingSphere que abarca el sphere en su punto de origen mas el sphere en su punto final deseado</param>
/// <param name="t">Menor instante de colision, en Elipsoid space</param>
/// <param name="q">Punto mas cercano de colision, en Elipsoid space</param>
/// <param name="n">Vector normal de la superficie contra la que se colisiona</param>
/// <returns>True si hay colision</returns>
public override bool intersectMovingElipsoid(TgcBoundingSphere eSphere, Vector3 eMovementVector, Vector3 eRadius, TgcBoundingSphere movementSphere, out float t, out Vector3 q, out Vector3 n)
{
//Pasar AABB a Elipsoid Space
eAABB.setExtremes(
TgcVectorUtils.div(aabb.PMin, eRadius),
TgcVectorUtils.div(aabb.PMax, eRadius)
);
t = -1f;
q = Vector3.Empty;
n = Vector3.Empty;
// Compute the AABB resulting from expanding b by sphere radius r
TgcBoundingBox.AABBStruct e = eAABB.toStruct();
e.min.X -= eSphere.Radius; e.min.Y -= eSphere.Radius; e.min.Z -= eSphere.Radius;
e.max.X += eSphere.Radius; e.max.Y += eSphere.Radius; e.max.Z += eSphere.Radius;
// Intersect ray against expanded AABB e. Exit with no intersection if ray
// misses e, else get intersection point p and time t as result
Vector3 p;
TgcRay.RayStruct ray = new TgcRay.RayStruct();
ray.origin = eSphere.Center;
ray.direction = eMovementVector;
if (!intersectRayAABB(ray, e, out t, out p) || t > 1.0f)
{
return(false);
}
// Compute which min and max faces of b the intersection point p lies
// outside of. Note, u and v cannot have the same bits set and
// they must have at least one bit set among them
int i = 0;
int[] sign = new int[3];
if (p.X < eAABB.PMin.X)
{
sign[0] = -1;
i++;
}
if (p.X > eAABB.PMax.X)
{
sign[0] = 1;
i++;
}
if (p.Y < eAABB.PMin.Y)
{
sign[1] = -1;
i++;
}
if (p.Y > eAABB.PMax.Y)
{
sign[1] = 1;
i++;
}
if (p.Z < eAABB.PMin.Z)
{
sign[2] = -1;
i++;
}
if (p.Z > eAABB.PMax.Z)
{
sign[2] = 1;
i++;
}
//Face
if (i == 1)
{
n = new Vector3(sign[0], sign[1], sign[2]);
q = eSphere.Center + t * eMovementVector - eSphere.Radius * n;
return(true);
}
// Define line segment [c, c+d] specified by the sphere movement
Segment seg = new Segment(eSphere.Center, eSphere.Center + eMovementVector);
//Box extent and center
Vector3 extent = eAABB.calculateAxisRadius();
Vector3 center = eAABB.PMin + extent;
//Edge
if (i == 2)
{
//Generar los dos puntos extremos del Edge
float[] extentDir = new float[] { sign[0], sign[1], sign[2] };
int zeroIndex = sign[0] == 0 ? 0 : (sign[1] == 0 ? 1 : 2);
extentDir[zeroIndex] = 1;
Vector3 capsuleA = center + new Vector3(extent.X * extentDir[0], extent.Y * extentDir[1], extent.Z * extentDir[2]);
extentDir[zeroIndex] = -1;
Vector3 capsuleB = center + new Vector3(extent.X * extentDir[0], extent.Y * extentDir[1], extent.Z * extentDir[2]);
//Colision contra el Edge hecho Capsula
if (intersectSegmentCapsule(seg, new Capsule(capsuleA, capsuleB, eSphere.Radius), out t))
{
n = new Vector3(sign[0], sign[1], sign[2]);
n.Normalize();
q = eSphere.Center + t * eMovementVector - eSphere.Radius * n;
return(true);
}
}
//Vertex
if (i == 3)
{
float tmin = float.MaxValue;
Vector3 capsuleA = center + new Vector3(extent.X * sign[0], extent.Y * sign[1], extent.Z * sign[2]);
Vector3 capsuleB;
capsuleB = center + new Vector3(extent.X * -sign[0], extent.Y * sign[1], extent.Z * sign[2]);
if (intersectSegmentCapsule(seg, new Capsule(capsuleA, capsuleB, eSphere.Radius), out t))
{
tmin = TgcCollisionUtils.min(t, tmin);
}
capsuleB = center + new Vector3(extent.X * sign[0], extent.Y * -sign[1], extent.Z * sign[2]);
if (intersectSegmentCapsule(seg, new Capsule(capsuleA, capsuleB, eSphere.Radius), out t))
{
tmin = TgcCollisionUtils.min(t, tmin);
}
capsuleB = center + new Vector3(extent.X * sign[0], extent.Y * sign[1], extent.Z * -sign[2]);
if (intersectSegmentCapsule(seg, new Capsule(capsuleA, capsuleB, eSphere.Radius), out t))
{
tmin = TgcCollisionUtils.min(t, tmin);
}
if (tmin == float.MaxValue)
{
return(false); // No intersection
}
t = tmin;
n = new Vector3(sign[0], sign[1], sign[2]);
n.Normalize();
q = eSphere.Center + t * eMovementVector - eSphere.Radius * n;
return(true); // Intersection at time t == tmin
}
return(false);
}
