/// <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 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; }
/// <summary> /// Interseccion Ray-OBB. /// Devuelve true y el punto q de colision si hay interseccion. /// </summary> public static bool intersectRayObb(TgcRay ray, TgcObb obb, out Vector3 q) { //Transformar Ray a OBB-space Vector3 a = ray.Origin; Vector3 b = ray.Origin + ray.Direction; a = obb.toObbSpace(a); b = obb.toObbSpace(b); TgcRay.RayStruct ray2 = new TgcRay.RayStruct(); ray2.origin = a; ray2.direction = Vector3.Normalize(b - a); //Crear AABB que representa al OBB Vector3 min = -obb.Extents; Vector3 max = obb.Extents; TgcBoundingBox.AABBStruct aabb = new TgcBoundingBox.AABBStruct(); aabb.min = min; aabb.max = max; //Hacer interseccion Ray-AABB if (TgcCollisionUtils.intersectRayAABB(ray2, aabb, out q)) { //Pasar q a World-Space q = obb.toWorldSpace(q); return true; } return false; }