/// <summary> /// Indica si un Ray colisiona con un BoundingSphere. /// Si el resultado es True se carga el punto de colision (q) y la distancia de colision en el Ray (t). /// La dirección del Ray debe estar normalizada. /// </summary> /// <param name="ray">Ray</param> /// <param name="sphere">BoundingSphere</param> /// <param name="t">Distancia de colision del Ray</param> /// <param name="q">Punto de colision</param> /// <returns>True si hay colision</returns> public static bool intersectRaySphere(TgcRay ray, TgcBoundingSphere sphere, out float t, out Vector3 q) { t = -1; q = Vector3.Empty; Vector3 m = ray.Origin - sphere.Center; float b = Vector3.Dot(m, ray.Direction); float c = Vector3.Dot(m, m) - sphere.Radius * sphere.Radius; // Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0) if (c > 0.0f && b > 0.0f) { return(false); } float discr = b * b - c; // A negative discriminant corresponds to ray missing sphere if (discr < 0.0f) { return(false); } // Ray now found to intersect sphere, compute smallest t value of intersection t = -b - FastMath.Sqrt(discr); // If t is negative, ray started inside sphere so clamp t to zero if (t < 0.0f) { t = 0.0f; } q = ray.Origin + t * ray.Direction; return(true); }
// /// <summary> /// Indica si un BoundingSphere colisiona con un Ray (sin indicar su punto de colision) /// La dirección del Ray debe estar normalizada. /// </summary> /// <param name="ray">Ray</param> /// <param name="sphere">BoundingSphere</param> /// <returns>True si hay colision</returns> public static bool testRaySphere(TgcRay ray, TgcBoundingSphere sphere) { Vector3 m = ray.Origin - sphere.Center; float c = Vector3.Dot(m, m) - sphere.Radius * sphere.Radius; // If there is definitely at least one real root, there must be an intersection if (c <= 0.0f) { return(true); } float b = Vector3.Dot(m, ray.Direction); // Early exit if ray origin outside sphere and ray pointing away from sphere if (b > 0.0f) { return(false); } float disc = b * b - c; // A negative discriminant corresponds to ray missing sphere if (disc < 0.0f) { return(false); } // Now ray must hit sphere return(true); }
public override bool intersectRay(TgcRay ray, Matrix transform, out Vector3 q) { Vector3[] v = new Vector3[vertices.Count]; for (int i = 0; i < v.Length; i++) { v[i] = Vector3.TransformCoordinate(vertices[i].position, transform); } float t; return TgcCollisionUtils.intersectRayConvexPolygon(ray, v, out t, out q); }
public override bool intersectRay(TgcRay ray, Matrix transform, out Vector3 q) { //Actualizar OBB con posiciones de la arista para utilizar en colision EditablePolyUtils.updateObbFromSegment(COLLISION_OBB, Vector3.TransformCoordinate(a.position, transform), Vector3.TransformCoordinate(b.position, transform), 0.4f); //ray-obb return TgcCollisionUtils.intersectRayObb(ray, COLLISION_OBB, out q); }
/// <summary> /// Indica si un Ray colisiona con un Plano. /// Tanto la normal del plano como la dirección del Ray se asumen normalizados. /// </summary> /// <param name="ray">Ray a testear</param> /// <param name="plane">Plano a testear</param> /// <param name="t">Instante de colisión</param> /// <param name="q">Punto de colisión con el plano</param> /// <returns>True si hubo colisión</returns> public static bool intersectRayPlane(TgcRay ray, Plane plane, out float t, out Vector3 q) { Vector3 planeNormal = TgcCollisionUtils.getPlaneNormal(plane); float numer = plane.Dot(ray.Origin); float denom = Vector3.Dot(planeNormal, ray.Direction); t = -numer / denom; if (t > 0.0f) { q = ray.Origin + ray.Direction * t; return(true); } q = Vector3.Empty; return(false); }
/// <summary> /// Indica si un segmento de recta colisiona con un BoundingSphere. /// Si el resultado es True se carga el punto de colision (q) y la distancia de colision en el t. /// La dirección del Ray debe estar normalizada. /// </summary> /// <param name="p0">Punto inicial del segmento</param> /// <param name="p1">Punto final del segmento</param> /// <param name="s">BoundingSphere</param> /// <param name="t">Distancia de colision del segmento</param> /// <param name="q">Punto de colision</param> /// <returns>True si hay colision</returns> public static bool intersectSegmentSphere(Vector3 p0, Vector3 p1, TgcBoundingSphere sphere, out float t, out Vector3 q) { Vector3 segmentDir = p1 - p0; TgcRay ray = new TgcRay(p0, segmentDir); if (TgcCollisionUtils.intersectRaySphere(ray, sphere, out t, out q)) { float segmentLengthSq = segmentDir.LengthSq(); Vector3 collisionDiff = q - p0; float collisionLengthSq = collisionDiff.LengthSq(); if (collisionLengthSq <= segmentLengthSq) { return(true); } } return(false); }
/// <summary> /// Indica si un Ray colisiona con un AABB. /// Si hay intersección devuelve True, q contiene /// el punto de intesección. /// Basado en el código de: http://www.codercorner.com/RayAABB.cpp /// La dirección del Ray puede estar sin normalizar. /// </summary> /// <param name="ray">Ray</param> /// <param name="a">AABB</param> /// <param name="q">Punto de intersección</param> /// <returns>True si hay colisión</returns> public static bool intersectRayAABB(TgcRay ray, TgcBoundingBox aabb, out Vector3 q) { float t; return TgcCollisionUtils.intersectRayAABB(ray.toStruct(), aabb.toStruct(), out q); }
/// <summary> /// Detecta colision entre un segmento pq y un triangulo abc. /// Devuelve true si hay colision y carga las coordenadas barycentricas (u,v,w) de la colision, el /// instante t de colision y el punto c de colision. /// Basado en paper Tomas Moller: http://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm /// </summary> /// <param name="ray">Ray</param> /// <param name="a">Vertice 1 del triangulo</param> /// <param name="b">Vertice 2 del triangulo</param> /// <param name="c">Vertice 3 del triangulo</param> /// <param name="t">Instante de colision</param> /// <param name="q">Punto de colision</param> /// <returns>True si hay colision</returns> public static bool intersectRayTriangle(TgcRay ray, Vector3 v1, Vector3 v2, Vector3 v3, out float t, out Vector3 q) { q = Vector3.Empty; t = -1; Vector3 e1, e2; //Edge1, Edge2 Vector3 P, Q, T; float det, inv_det, u, v; //Find vectors for two edges sharing V1 e1 = v2 - v1; e2 = v3 - v1; //Begin calculating determinant - also used to calculate u parameter P = Vector3.Cross(ray.Direction, e2); //if determinant is near zero, ray lies in plane of triangle det = Vector3.Dot(e1, P); //NOT CULLING if(det > -float.Epsilon && det < float.Epsilon) return false; inv_det = 1.0f / det; //calculate distance from V1 to ray origin T = ray.Origin - v1; //Calculate u parameter and test bound u = Vector3.Dot(T, P) * inv_det; //The intersection lies outside of the triangle if(u < 0.0f || u > 1.0f) return false; //Prepare to test v parameter Q = Vector3.Cross(T, e1); //Calculate V parameter and test bound v = Vector3.Dot(ray.Direction, Q) * inv_det; //The intersection lies outside of the triangle if(v < 0.0f || u + v > 1.0f) return false; t = Vector3.Dot(e2, Q) * inv_det; if(t > float.Epsilon) { //ray intersection q = ray.Origin + t * ray.Direction; return true; } // No hit, no win return false; }
/// <summary> /// Indica si un Ray colisiona con un AABB. /// Si hay intersección devuelve True, q contiene /// el punto de intesección. /// Basado en el código de: http://www.codercorner.com/RayAABB.cpp /// La dirección del Ray puede estar sin normalizar. /// </summary> /// <param name="ray">Ray</param> /// <param name="a">AABB</param> /// <param name="q">Punto de intersección</param> /// <returns>True si hay colisión</returns> public static bool intersectRayAABB(TgcRay ray, TgcBoundingBox aabb, out Vector3 q) { q = Vector3.Empty; bool inside = true; float[] aabbMin = toArray(aabb.PMin); float[] aabbMax = toArray(aabb.PMax); float[] rayOrigin = toArray(ray.Origin); float[] rayDir = toArray(ray.Direction); float[] max_t = new float[3] { -1.0f, -1.0f, -1.0f }; float[] coord = new float[3]; for (uint i = 0; i < 3; ++i) { if (rayOrigin[i] < aabbMin[i]) { inside = false; coord[i] = aabbMin[i]; if (rayDir[i] != 0.0f) { max_t[i] = (aabbMin[i] - rayOrigin[i]) / rayDir[i]; } } else if (rayOrigin[i] > aabbMax[i]) { inside = false; coord[i] = aabbMax[i]; if (rayDir[i] != 0.0f) { max_t[i] = (aabbMax[i] - rayOrigin[i]) / rayDir[i]; } } } // If the Ray's start position is inside the Box, we can return true straight away. if (inside) { q = toVector3(rayOrigin); return(true); } uint plane = 0; if (max_t[1] > max_t[plane]) { plane = 1; } if (max_t[2] > max_t[plane]) { plane = 2; } if (max_t[plane] < 0.0f) { return(false); } for (uint i = 0; i < 3; ++i) { if (plane != i) { coord[i] = rayOrigin[i] + max_t[plane] * rayDir[i]; if (coord[i] < aabbMin[i] - float.Epsilon || coord[i] > aabbMax[i] + float.Epsilon) { return(false); } } } q = toVector3(coord); return(true); }
public override bool intersectRay(TgcRay ray, Matrix transform, out Vector3 q) { COLLISION_SPHERE.setCenter(Vector3.TransformCoordinate(position, transform)); float t; return TgcCollisionUtils.intersectRaySphere(ray, COLLISION_SPHERE, out t, out q); }
public override Boolean Intercepts(TgcRay ray) { return TgcCollisionUtils.testRaySphere(ray, Sphere); }
public override Boolean Intercepts(TgcRay ray) { var v = new Vector3(); return TgcCollisionUtils.intersectRayObb(ray, Obb, out v); }
/// <summary> /// Retorna true si hubo interseccion con el terreno y setea el collisionPoint. /// </summary> /// <param name="ray"></param> /// <param name="collisionPoint"></param> /// <returns></returns> public bool intersectRay(TgcRay ray, out Vector3 collisionPoint) { collisionPoint = Vector3.Empty; Matrix scaleInv = Matrix.Scaling(new Vector3(1/ScaleXZ, 1/ScaleY, 1/ScaleXZ)); Vector3 a = Vector3.TransformCoordinate(ray.Origin, scaleInv) - traslation; Vector3 r = Vector3.TransformCoordinate(ray.Direction, scaleInv); if (a.Y < minIntensity) return false; Vector3 q; //Me fijo si intersecta con el BB del terreno. if (!TgcCollisionUtils.intersectRayAABB(new TgcRay(a, r).toStruct(), aabb.toStruct(), out q)) return false; float minT=0; //Obtengo el T de la interseccion. if (q != a) { if (r.X != 0) minT = (q.X - a.X) / r.X; else if (r.Y != 0) minT = (q.Y - a.Y) / r.Y; else if (r.Z != 0) minT = (q.Z - a.Z) / r.Z; } //Me desplazo por el rayo hasta que su altura sea menor a la del terreno en ese punto //o me salga del AABB. float t=0; float step = 1; for (t = minT; ; t += step) { collisionPoint = a + t * r; float y; if(!interpoledIntensity(collisionPoint.X, collisionPoint.Z, out y)) return false; if (collisionPoint.Y <= y + float.Epsilon) { collisionPoint.Y = y; collisionPoint = Vector3.TransformCoordinate(collisionPoint + traslation, Matrix.Scaling(ScaleXZ, ScaleY, ScaleXZ)); return true; } } }
/// <summary> /// Selecciona el eje actual del gizmo haciendo picking /// </summary> public void selectAxisByPicking(TgcRay ray) { this.selectedAxis = doPickAxis(ray); }
/// <summary> /// Hacer picking contra todos los ejes y devolver el eje seleccionado (si hay colision). /// Tambien se evaluan los ejes compuestos (XY, XZ, YZ) /// </summary> public Axis doPickAxis(TgcRay ray) { Vector3 collP; if (TgcCollisionUtils.intersectRayAABB(ray, boxX.BoundingBox, out collP)) { return Axis.X; } else if (TgcCollisionUtils.intersectRayAABB(ray, boxY.BoundingBox, out collP)) { return Axis.Y; } else if (TgcCollisionUtils.intersectRayAABB(ray, boxZ.BoundingBox, out collP)) { return Axis.Z; } else if (TgcCollisionUtils.intersectRayAABB(ray, boxXZ.BoundingBox, out collP)) { return Axis.XZ; } else if (TgcCollisionUtils.intersectRayAABB(ray, boxXY.BoundingBox, out collP)) { return Axis.XY; } else if (TgcCollisionUtils.intersectRayAABB(ray, boxYZ.BoundingBox, out collP)) { return Axis.YZ; } else { return Axis.None; } }
public Vector3 intersectRayTerrain(TgcRay ray) { int iteraciones = heightmapResolution/cantidadFilasColumnas * (int)currentScaleXZ; Vector3 dir = ray.Direction; dir.Normalize(); Vector3 origin = ray.Origin; Vector3 pos = origin; float y = 0; for (int i = 0; i < iteraciones; i++) { interpoledHeight(pos.X, pos.Z, out y); if (FastMath.Abs(pos.Y - y) < 1f) { return pos; } pos += dir; } return pos; }
public void fireSniper() { //Disparamos el arma, nos fijamos si colisiona con un enemigo, y si hay obstaculos en el medio Vector3 dir = CustomFpsCamera.Instance.LookAt - CustomFpsCamera.Instance.Position; TgcRay ray = new TgcRay(CustomFpsCamera.Instance.Position, dir); Vector3 newPosition = new Vector3(0, 0, 0); List<Vector3> posicionObstaculos = new List<Vector3>(); bool vegetacionFrenoDisparo = false; foreach (TgcMesh obstaculo in vegetation) { if (TgcCollisionUtils.intersectRayAABB(ray, obstaculo.BoundingBox, out newPosition)) posicionObstaculos.Add(newPosition); } int killHeadTracker = 0; bool hit = false; for (int i = enemies.Count - 1; i >= 0; i--) { if (TgcCollisionUtils.intersectRayAABB(ray, enemies[i].HEADSHOT_BOUNDINGBOX, out newPosition)) { foreach(Vector3 posicion in posicionObstaculos){ if (Vector3.Length(posicion - ray.Origin) < Vector3.Length(newPosition - ray.Origin)) { vegetacionFrenoDisparo = true; break; } } if (!vegetacionFrenoDisparo) { hit = true; score += 1; killHeadTracker++; HUDManager.Instance.headShot(); enemies[i].health = 0; enemies[i].sangrar(-dir, newPosition.Y - enemies[i].getPosicionActual().Y); //eliminarEnemigo(enemies[i]); enemies[i].morirse(); sumarScore(enemies[i]); } vegetacionFrenoDisparo = false; } if (!hit && TgcCollisionUtils.intersectRayAABB(ray, enemies[i].LEGS_BOUNDINGBOX, out newPosition)) { foreach (Vector3 posicion in posicionObstaculos) { if (Vector3.Length(posicion - ray.Origin) < Vector3.Length(newPosition - ray.Origin)) { vegetacionFrenoDisparo = true; break; } } if (!vegetacionFrenoDisparo) { enemies[i].health -= 25; enemies[i].sangrar(-dir, newPosition.Y - enemies[i].getPosicionActual().Y); hit = true; if (enemies[i].health <= 0) { // eliminarEnemigo(enemies[i]); enemies[i].morirse(); sumarScore(enemies[i]); } } vegetacionFrenoDisparo = false; } if (!hit && TgcCollisionUtils.intersectRayAABB(ray, enemies[i].CHEST_BOUNDINGBOX, out newPosition)) { foreach(Vector3 posicion in posicionObstaculos){ if (Vector3.Length(posicion - ray.Origin) < Vector3.Length(newPosition - ray.Origin)) { vegetacionFrenoDisparo = true; break; } } if (!vegetacionFrenoDisparo) { enemies[i].health -= 50; enemies[i].sangrar(-dir, newPosition.Y - enemies[i].getPosicionActual().Y); if (enemies[i].health <= 0) { //eliminarEnemigo(enemies[i]); enemies[i].morirse(); sumarScore(enemies[i]); } } vegetacionFrenoDisparo = false; } hit = false; } //////////////////////////disparo a barriles//////////////////////////////////////// for (int i = barriles.Count - 1; i >= 0; i--) { if (TgcCollisionUtils.intersectRayAABB(ray, barriles[i].mesh.BoundingBox, out newPosition)) { foreach (Vector3 posicion in posicionObstaculos) { if (Vector3.Length(posicion - ray.Origin) < Vector3.Length(newPosition - ray.Origin)) { vegetacionFrenoDisparo = true; break; } } if (!vegetacionFrenoDisparo) { // playSound(explosionSoundDir); TODO barriles[i].explotar(); } vegetacionFrenoDisparo = false; } } ///////////////////////////////////////////////////////////// if (killHeadTracker > 1) { HUDManager.Instance.headHunter();//Constante que reproduce el efecto de headhunter. score += killHeadTracker; HUDManager.Instance.refreshScore(); } }
public Vector3 fireLauncher() { //Disparamos el arma, nos fijamos si colisiona con un enemigo, y si hay obstaculos en el medio TgcRay ray = new TgcRay(CustomFpsCamera.Instance.Position, CustomFpsCamera.Instance.LookAt - CustomFpsCamera.Instance.Position); Vector3 newPosition = new Vector3(0, 0, 0); List<Vector3> posicionObstaculos = new List<Vector3>(); foreach (TgcMesh obstaculo in vegetation) { if (TgcCollisionUtils.intersectRayAABB(ray, obstaculo.BoundingBox, out newPosition)) posicionObstaculos.Add(newPosition); } for (int i = enemies.Count - 1; i >= 0; i--) { if (TgcCollisionUtils.intersectRayAABB(ray, enemies[i].mesh.BoundingBox, out newPosition)) posicionObstaculos.Add(newPosition); } //////////////////////////disparo a barriles//////////////////////////////////////// for (int i = barriles.Count - 1; i >= 0; i--) { if (TgcCollisionUtils.intersectRayAABB(ray, barriles[i].mesh.BoundingBox, out newPosition)) posicionObstaculos.Add(newPosition); } posicionObstaculos.Add(intersectRayTerrain(ray)); posicionObstaculos.Sort(delegate(Vector3 x, Vector3 y) { return distanciaACamara(x).CompareTo(distanciaACamara(y)); }); Vector3 min = posicionObstaculos[0]; return min; }
/// <summary> /// Indica si un Ray colisiona con un AABB. /// Si hay intersección devuelve True, q contiene /// el punto de intesección. /// Basado en el código de: http://www.codercorner.com/RayAABB.cpp /// La dirección del Ray puede estar sin normalizar. /// </summary> /// <param name="ray">Ray</param> /// <param name="a">AABB</param> /// <param name="q">Punto de intersección</param> /// <returns>True si hay colisión</returns> public static bool intersectRayAABB(TgcRay.RayStruct ray, TgcBoundingBox.AABBStruct aabb, out Vector3 q) { q = Vector3.Empty; bool inside = true; float[] aabbMin = toArray(aabb.min); float[]aabbMax = toArray(aabb.max); float[]rayOrigin = toArray(ray.origin); float[]rayDir = toArray(ray.direction); float[] max_t = new float[3]{-1.0f, -1.0f, -1.0f}; float[] coord = new float[3]; for (uint i = 0; i < 3; ++i) { if (rayOrigin[i] < aabbMin[i]) { inside = false; coord[i] = aabbMin[i]; if (rayDir[i] != 0.0f) { max_t[i] = (aabbMin[i] - rayOrigin[i]) / rayDir[i]; } } else if (rayOrigin[i] > aabbMax[i]) { inside = false; coord[i] = aabbMax[i]; if (rayDir[i] != 0.0f) { max_t[i] = (aabbMax[i] - rayOrigin[i]) / rayDir[i]; } } } // If the Ray's start position is inside the Box, we can return true straight away. if (inside) { q = toVector3(rayOrigin); return true; } uint plane = 0; if (max_t[1] > max_t[plane]) { plane = 1; } if (max_t[2] > max_t[plane]) { plane = 2; } if (max_t[plane] < 0.0f) { return false; } for (uint i = 0; i < 3; ++i) { if (plane != i) { coord[i] = rayOrigin[i] + max_t[plane] * rayDir[i]; if (coord[i] < aabbMin[i] - float.Epsilon || coord[i] > aabbMax[i] + float.Epsilon) { return false; } } } q = toVector3(coord); return true; }
/// <summary> /// Detección de colisiones recursiva /// </summary> public void doCollideWithWorld(TgcBoundingSphere characterSphere, Vector3 movementVector, List<TgcBoundingBox> obstaculos, int recursionDepth) { //Limitar recursividad if (recursionDepth > 5) { return; } //Ver si la distancia a recorrer es para tener en cuenta float distanceToTravelSq = movementVector.LengthSq(); if (distanceToTravelSq < EPSILON) { return; } //Posicion deseada Vector3 originalSphereCenter = characterSphere.Center; Vector3 nextSphereCenter = originalSphereCenter + movementVector; //Buscar el punto de colision mas cercano de todos los objetos candidatos float minCollisionDistSq = float.MaxValue; Vector3 realMovementVector = movementVector; TgcBoundingBox.Face collisionFace = null; TgcBoundingBox collisionObstacle = null; Vector3 nearestPolygonIntersectionPoint = Vector3.Empty; foreach (TgcBoundingBox obstaculoBB in obstaculos) { //Obtener los polígonos que conforman las 6 caras del BoundingBox TgcBoundingBox.Face[] bbFaces = obstaculoBB.computeFaces(); foreach (TgcBoundingBox.Face bbFace in bbFaces) { Vector3 pNormal = TgcCollisionUtils.getPlaneNormal(bbFace.Plane); TgcRay movementRay = new TgcRay(originalSphereCenter, movementVector); float brutePlaneDist; Vector3 brutePlaneIntersectionPoint; if (!TgcCollisionUtils.intersectRayPlane(movementRay, bbFace.Plane, out brutePlaneDist, out brutePlaneIntersectionPoint)) { continue; } float movementRadiusLengthSq = Vector3.Multiply(movementVector, characterSphere.Radius).LengthSq(); if (brutePlaneDist * brutePlaneDist > movementRadiusLengthSq) { continue; } //Obtener punto de colisión en el plano, según la normal del plano float pDist; Vector3 planeIntersectionPoint; Vector3 sphereIntersectionPoint; TgcRay planeNormalRay = new TgcRay(originalSphereCenter, -pNormal); bool embebbed = false; bool collisionFound = false; if (TgcCollisionUtils.intersectRayPlane(planeNormalRay, bbFace.Plane, out pDist, out planeIntersectionPoint)) { //Ver si el plano está embebido en la esfera if (pDist <= characterSphere.Radius) { embebbed = true; //TODO: REVISAR ESTO, caso embebido a analizar con más detalle sphereIntersectionPoint = originalSphereCenter - pNormal * characterSphere.Radius; } //Esta fuera de la esfera else { //Obtener punto de colisión del contorno de la esfera según la normal del plano sphereIntersectionPoint = originalSphereCenter - Vector3.Multiply(pNormal, characterSphere.Radius); //Disparar un rayo desde el contorno de la esfera hacia el plano, con el vector de movimiento TgcRay sphereMovementRay = new TgcRay(sphereIntersectionPoint, movementVector); if (!TgcCollisionUtils.intersectRayPlane(sphereMovementRay, bbFace.Plane, out pDist, out planeIntersectionPoint)) { //no hay colisión continue; } } //Ver si planeIntersectionPoint pertenece al polígono Vector3 newMovementVector; float newMoveDistSq; Vector3 polygonIntersectionPoint; if (pointInBounbingBoxFace(planeIntersectionPoint, bbFace)) { if (embebbed) { //TODO: REVISAR ESTO, nunca debería pasar //throw new Exception("El polígono está dentro de la esfera"); } polygonIntersectionPoint = planeIntersectionPoint; collisionFound = true; } else { //Buscar el punto mas cercano planeIntersectionPoint que tiene el polígono real de esta cara polygonIntersectionPoint = TgcCollisionUtils.closestPointRectangle3d(planeIntersectionPoint, bbFace.Extremes[0], bbFace.Extremes[1], bbFace.Extremes[2]); //Revertir el vector de velocidad desde el nuevo polygonIntersectionPoint para ver donde colisiona la esfera, si es que llega Vector3 reversePointSeg = polygonIntersectionPoint - movementVector; if (TgcCollisionUtils.intersectSegmentSphere(polygonIntersectionPoint, reversePointSeg, characterSphere, out pDist, out sphereIntersectionPoint)) { collisionFound = true; } } if (collisionFound) { //Nuevo vector de movimiento acotado newMovementVector = polygonIntersectionPoint - sphereIntersectionPoint; newMoveDistSq = newMovementVector.LengthSq(); if (newMoveDistSq <= distanceToTravelSq && newMoveDistSq < minCollisionDistSq) { minCollisionDistSq = newMoveDistSq; realMovementVector = newMovementVector; nearestPolygonIntersectionPoint = polygonIntersectionPoint; collisionFace = bbFace; collisionObstacle = obstaculoBB; } } } } } //Si nunca hubo colisión, avanzar todo lo requerido if (collisionFace == null) { //Avanzar hasta muy cerca float movementLength = movementVector.Length(); movementVector.Multiply((movementLength - EPSILON) / movementLength); characterSphere.moveCenter(movementVector); return; } //Solo movernos si ya no estamos muy cerca if (minCollisionDistSq >= EPSILON) { //Mover el BoundingSphere hasta casi la nueva posición real float movementLength = realMovementVector.Length(); realMovementVector.Multiply((movementLength - EPSILON) / movementLength); characterSphere.moveCenter(realMovementVector); } //Calcular plano de Sliding Vector3 slidePlaneOrigin = nearestPolygonIntersectionPoint; Vector3 slidePlaneNormal = characterSphere.Center - nearestPolygonIntersectionPoint; slidePlaneNormal.Normalize(); Plane slidePlane = Plane.FromPointNormal(slidePlaneOrigin, slidePlaneNormal); //Proyectamos el punto original de destino en el plano de sliding TgcRay slideRay = new TgcRay(nearestPolygonIntersectionPoint + Vector3.Multiply(movementVector, slideFactor), slidePlaneNormal); float slideT; Vector3 slideDestinationPoint; if (TgcCollisionUtils.intersectRayPlane(slideRay, slidePlane, out slideT, out slideDestinationPoint)) { //Nuevo vector de movimiento Vector3 slideMovementVector = slideDestinationPoint - nearestPolygonIntersectionPoint; if (slideMovementVector.LengthSq() < EPSILON) { return; } //Recursividad para aplicar sliding doCollideWithWorld(characterSphere, slideMovementVector, obstaculos, recursionDepth + 1); } }
/// <summary> /// Indica si un Ray colisiona con un Plano. /// Tanto la normal del plano como la dirección del Ray se asumen normalizados. /// </summary> /// <param name="ray">Ray a testear</param> /// <param name="plane">Plano a testear</param> /// <param name="t">Instante de colisión</param> /// <param name="q">Punto de colisión con el plano</param> /// <returns>True si hubo colisión</returns> public static bool intersectRayPlane(TgcRay ray, Plane plane, out float t, out Vector3 q) { Vector3 planeNormal = TgcCollisionUtils.getPlaneNormal(plane); float numer = plane.Dot(ray.Origin); float denom = Vector3.Dot(planeNormal, ray.Direction); t = -numer / denom; if(t > 0.0f) { q = ray.Origin + ray.Direction * t; return true; } q = Vector3.Empty; return false; }
/// <summary> /// Picking con los planos XZ e YZ ubicados en el centro del objeto /// </summary> public Vector3 getPickingZ(TgcRay ray, Vector3 objCenter) { //Mover ambos planos hacia el centro del objeto pickingXZAabb.setExtremes( new Vector3(pickingXZAabb.PMin.X, objCenter.Y - SMALL_VAL, pickingXZAabb.PMin.Z), new Vector3(pickingXZAabb.PMax.X, objCenter.Y, pickingXZAabb.PMax.Z)); pickingYZAabb.setExtremes( new Vector3(objCenter.X - SMALL_VAL, pickingYZAabb.PMin.Y, pickingYZAabb.PMin.Z), new Vector3(objCenter.X, pickingYZAabb.PMax.Y, pickingYZAabb.PMax.Z)); Vector3 q1, q2; bool r1, r2; r1 = TgcCollisionUtils.intersectRayAABB(ray, pickingXZAabb, out q1); r2 = TgcCollisionUtils.intersectRayAABB(ray, pickingYZAabb, out q2); if (r1 && r2) { Vector2 objPos = new Vector2(objCenter.X, objCenter.Y); float diff1 = Vector2.Length(new Vector2(q1.X, q1.Y) - objPos); float diff2 = Vector2.Length(new Vector2(q2.X, q2.Y) - objPos); return diff1 < diff2 ? q1 : q2; } else if (r1) return clampPickingResult(q1); else if (r2) return clampPickingResult(q2); return objCenter; }
/// <summary> /// Retorna true si hubo interseccion con el plano del terreno y setea el collisionPoint con la altura en ese punto. /// </summary> /// <param name="ray"></param> /// <param name="collisionPoint"></param> /// <returns></returns> public bool intersectRayPlane(TgcRay ray, out Vector3 collisionPoint) { collisionPoint = Vector3.Empty; float minHeight = (minIntensity + traslation.Y) * ScaleY; float t; //Me fijo si intersecta con el BB del terreno. if (!TgcCollisionUtils.intersectRayPlane(ray, new Plane(0, 1, 0, -minHeight), out t, out collisionPoint)) return false; return interpoledHeight(collisionPoint.X, collisionPoint.Z, out collisionPoint.Y); }
/// <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; Vector3 origin = ray.origin; Vector3 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 wall 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. Vector3 U = capsule.segment.dir; Vector3 V = U; Vector3 W = U; generateComplementBasis(ref U, ref V, W); float rSqr = capsule.radius * capsule.radius; float extent = capsule.segment.extent; // Convert incoming line origin to capsule coordinates. Vector3 diff = origin - capsule.segment.center; Vector3 P = new Vector3(Vector3.Dot(U, diff), Vector3.Dot(V, diff), Vector3.Dot(W, diff)); // Get the z-value, in capsule coordinates, of the incoming line's unit-length direction. float dz = Vector3.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. float 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 float 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. Vector3 D = new Vector3(Vector3.Dot(U, dir), Vector3.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 float a0 = P.X * P.X + P.Y * P.Y - rSqr; float a1 = P.X * D.X + P.Y * D.Y; float a2 = D.X * D.X + D.Y * D.Y; float discr = a1 * a1 - a0 * a2; if (discr < 0f) { // Line does not intersect infinite cylinder. return false; } float root, inv, tValue, zValue; int 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 wall 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. float 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; }
/// <summary> /// Picking con plano YZ ubicado en el centro del objeto /// </summary> public Vector3 getPickingYZ(TgcRay ray, Vector3 objCenter) { //Mover aabb en Y al centro del mesh pickingYZAabb.setExtremes( new Vector3(objCenter.X - SMALL_VAL, pickingYZAabb.PMin.Y, pickingYZAabb.PMin.Z), new Vector3(objCenter.X, pickingYZAabb.PMax.Y, pickingYZAabb.PMax.Z)); Vector3 q; bool r = TgcCollisionUtils.intersectRayAABB(ray, pickingYZAabb, out q); if (r) return clampPickingResult(q); return objCenter; }
/// <summary> /// Indica si un rayo colisiona con un cilindro. /// </summary> /// <param name="ray">Rayo</param> /// <param name="cylinder">Cilindro alineado</param> /// <returns>True si el rayo colisiona con el cilindro</returns> public static bool testRayCylinder(TgcRay ray, TgcFixedYBoundingCylinder cylinder) { Matrix transformation = cylinder.AntiTransformationMatrix; Vector3 origin = Vector3.TransformCoordinate(ray.Origin, transformation); Vector3 direction = Vector3.TransformNormal(ray.Direction, transformation); return TgcCollisionUtils.testRayCylinder(origin, direction); }
/// <summary> /// Interseccion entre un Ray y un AABB /// </summary> /// <param name="ray">Ray</param> /// <param name="aabb">AABB</param> /// <param name="tmin">Instante minimo de colision</param> /// <param name="q">Punto minimo de colision</param> /// <returns>True si hay colision</returns> private bool intersectRayAABB(TgcRay.RayStruct ray, TgcBoundingBox.AABBStruct aabb, out float tmin, out Vector3 q) { float[] aabbMin = TgcCollisionUtils.toArray(aabb.min); float[] aabbMax = TgcCollisionUtils.toArray(aabb.max); float[] p = TgcCollisionUtils.toArray(ray.origin); float[] d = TgcCollisionUtils.toArray(ray.direction); tmin = 0.0f; // set to -FLT_MAX to get first hit on line float tmax = float.MaxValue; // set to max distance ray can travel (for segment) q = Vector3.Empty; // For all three slabs for (int 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 float ood = 1.0f / d[i]; float t1 = (aabbMin[i] - p[i]) * ood; float 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; }
private bool canSeeWithObstacles(Vector3 targetPoint, List<ILevelObject> obstacles) { Vector3 pt = targetPoint-this.Position; TgcRay ray = new TgcRay(this.Position, pt); foreach (ILevelObject o in obstacles) { if (objectInsideRadius(pt.Length(), o)) { if (o.collidesWith(ray)) return false; } } return true; }
public TgcPickingRay() { ray = new TgcRay(); }
/// <summary> /// Indica si un Ray colisiona con un BoundingSphere. /// Si el resultado es True se carga el punto de colision (q) y la distancia de colision en el Ray (t). /// La dirección del Ray debe estar normalizada. /// </summary> /// <param name="ray">Ray</param> /// <param name="sphere">BoundingSphere</param> /// <param name="t">Distancia de colision del Ray</param> /// <param name="q">Punto de colision</param> /// <returns>True si hay colision</returns> public static bool intersectRaySphere(TgcRay ray, TgcBoundingSphere sphere, out float t, out Vector3 q) { t = -1; q = Vector3.Empty; Vector3 m = ray.Origin - sphere.Center; float b = Vector3.Dot(m, ray.Direction); float c = Vector3.Dot(m, m) - sphere.Radius * sphere.Radius; // Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0) if (c > 0.0f && b > 0.0f) return false; float discr = b*b - c; // A negative discriminant corresponds to ray missing sphere if (discr < 0.0f) return false; // Ray now found to intersect sphere, compute smallest t value of intersection t = -b - FastMath.Sqrt(discr); // If t is negative, ray started inside sphere so clamp t to zero if (t < 0.0f) t = 0.0f; q = ray.Origin + t * ray.Direction; return true; }
/// <summary> /// Detecta colision entre un rayo y un poligono convexo formado por un conjunto de vertices. /// </summary> /// <param name="ray">Rayo</param> /// <param name="polyVertices">Conjunto de vertices del poligono</param> /// <param name="t">Instante de tiempo de colision</param> /// <param name="q">Punto de colision</param> /// <returns>True si hay colision</returns> public static bool intersectRayConvexPolygon(TgcRay ray, Vector3[] polyVertices, out float t, out Vector3 q) { t = -1; q = Vector3.Empty; Vector3 v0 = polyVertices[0]; Vector3 v1 = polyVertices[1]; for (int i = 2; i < polyVertices.Length; i++) { Vector3 v2 = polyVertices[i]; if (intersectRayTriangle(ray, v0, v1, v2, out t, out q)) { return true; } v1 = v2; } return false; }
public Boolean Intercepts(TgcRay ray) { return _Colliders.Any(c => c.Intercepts(ray)); }
/// <summary> /// Indica si un segmento de recta colisiona con un BoundingSphere. /// Si el resultado es True se carga el punto de colision (q) y la distancia de colision en el t. /// La dirección del Ray debe estar normalizada. /// </summary> /// <param name="p0">Punto inicial del segmento</param> /// <param name="p1">Punto final del segmento</param> /// <param name="s">BoundingSphere</param> /// <param name="t">Distancia de colision del segmento</param> /// <param name="q">Punto de colision</param> /// <returns>True si hay colision</returns> public static bool intersectSegmentSphere(Vector3 p0, Vector3 p1, TgcBoundingSphere sphere, out float t, out Vector3 q) { Vector3 segmentDir = p1 - p0; TgcRay ray = new TgcRay(p0, segmentDir); if (TgcCollisionUtils.intersectRaySphere(ray, sphere, out t, out q)) { float segmentLengthSq = segmentDir.LengthSq(); Vector3 collisionDiff = q - p0; float collisionLengthSq = collisionDiff.LengthSq(); if (collisionLengthSq <= segmentLengthSq) { return true; } } 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; }
public PickingRay() { ray = new TgcRay(); }
// /// <summary> /// Indica si un BoundingSphere colisiona con un Ray (sin indicar su punto de colision) /// La dirección del Ray debe estar normalizada. /// </summary> /// <param name="ray">Ray</param> /// <param name="sphere">BoundingSphere</param> /// <returns>True si hay colision</returns> public static bool testRaySphere(TgcRay ray, TgcBoundingSphere sphere) { Vector3 m = ray.Origin - sphere.Center; float c = Vector3.Dot(m, m) - sphere.Radius * sphere.Radius; // If there is definitely at least one real root, there must be an intersection if (c <= 0.0f) return true; float b = Vector3.Dot(m, ray.Direction); // Early exit if ray origin outside sphere and ray pointing away from sphere if (b > 0.0f) return false; float disc = b * b - c; // A negative discriminant corresponds to ray missing sphere if (disc < 0.0f) return false; // Now ray must hit sphere return true; }