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