/// <summary>
/// Mover Elipsoide con detección de colisiones, sliding y gravedad.
/// Se actualiza la posición del centro del Elipsoide
/// </summary>
/// <param name="characterElipsoid">Elipsoide del cuerpo a mover</param>
/// <param name="movementVector">Movimiento a realizar</param>
/// <param name="colliders">Obstáculos contra los cuales se puede colisionar</param>
/// <returns>Desplazamiento relativo final efecutado al Elipsoide</returns>
public Vector3 moveCharacter(TgcElipsoid characterElipsoid, Vector3 movementVector, List<Collider> colliders)
{
//Guardar posicion original del Elipsoide
Vector3 originalElipsoidCenter = characterElipsoid.Center;
//Pasar elipsoid space
Vector3 eCenter = TgcVectorUtils.div(characterElipsoid.Center, characterElipsoid.Radius);
Vector3 eMovementVector = TgcVectorUtils.div(movementVector, characterElipsoid.Radius);
eSphere.setValues(eCenter, 1);
Vector3 eOrigCenter = eSphere.Center;
//Ver si la distancia a recorrer es para tener en cuenta
float distanceToTravelSq = movementVector.LengthSq();
if (distanceToTravelSq >= EPSILON)
{
//Mover la distancia pedida
selectPotentialColliders(characterElipsoid, movementVector, colliders);
this.result = doCollideWithWorld(eSphere, eMovementVector, characterElipsoid.Radius, objetosCandidatos, 0, movementSphere, 1);
}
//Aplicar gravedad
if (gravityEnabled)
{
//Mover con gravedad
Vector3 eGravity = TgcVectorUtils.div(gravityForce, characterElipsoid.Radius);
selectPotentialColliders(characterElipsoid, eGravity, colliders);
this.result = doCollideWithWorld(eSphere, eGravity, characterElipsoid.Radius, objetosCandidatos, 0, movementSphere, onGroundMinDotValue);
}
//Mover Elipsoid pasando valores de colision a R3
Vector3 movement = TgcVectorUtils.mul(eSphere.Center - eOrigCenter, characterElipsoid.Radius);
characterElipsoid.moveCenter(movement);
//Ajustar resultados
result.realMovmentVector = TgcVectorUtils.mul(result.realMovmentVector, characterElipsoid.Radius);
result.collisionPoint = TgcVectorUtils.mul(result.collisionPoint, characterElipsoid.Radius);
return movement;
}
/// <summary>
/// Actualizar eventos
/// </summary>
public void update()
{
TgcD3dInput input = GuiController.Instance.D3dInput;
TgcPickingRay pickingRay = editablePoly.Control.PickingRay;
switch (currentState)
{
case State.Init:
acumMovement = Vector3.Empty;
gizmoMesh.unSelect();
//Iniciar seleccion de eje
if (input.buttonDown(TgcD3dInput.MouseButtons.BUTTON_LEFT))
{
//Buscar colision con ejes del gizmo
pickingRay.updateRay();
gizmoMesh.selectAxisByPicking(pickingRay.Ray);
if (gizmoMesh.SelectedAxis == TranslateGizmoMesh.Axis.X)
{
initMouseP3d = editablePoly.Control.Grid.getPickingX(pickingRay.Ray, gizmoMesh.GizmoCenter);
}
else if (gizmoMesh.SelectedAxis == TranslateGizmoMesh.Axis.Y)
{
initMouseP3d = editablePoly.Control.Grid.getPickingY(pickingRay.Ray, gizmoMesh.GizmoCenter);
}
else if (gizmoMesh.SelectedAxis == TranslateGizmoMesh.Axis.Z)
{
initMouseP3d = editablePoly.Control.Grid.getPickingZ(pickingRay.Ray, gizmoMesh.GizmoCenter);
}
else if (gizmoMesh.SelectedAxis == TranslateGizmoMesh.Axis.XZ)
{
initMouseP3d = editablePoly.Control.Grid.getPickingXZ(pickingRay.Ray, gizmoMesh.GizmoCenter);
}
else if (gizmoMesh.SelectedAxis == TranslateGizmoMesh.Axis.XY)
{
initMouseP3d = editablePoly.Control.Grid.getPickingXY(pickingRay.Ray, gizmoMesh.GizmoCenter);
}
else if (gizmoMesh.SelectedAxis == TranslateGizmoMesh.Axis.YZ)
{
initMouseP3d = editablePoly.Control.Grid.getPickingYZ(pickingRay.Ray, gizmoMesh.GizmoCenter);
}
//Si eligio un eje, iniciar dragging
if (gizmoMesh.SelectedAxis != TranslateGizmoMesh.Axis.None)
{
currentState = State.Dragging;
}
else
{
if (input.buttonPressed(TgcD3dInput.MouseButtons.BUTTON_LEFT))
{
bool additive = input.keyDown(Microsoft.DirectX.DirectInput.Key.LeftControl) || input.keyDown(Microsoft.DirectX.DirectInput.Key.RightControl);
editablePoly.CurrentState = EditablePoly.State.SelectObject;
editablePoly.doDirectSelection(additive);
}
}
}
//Hacer mouse over sobre los ejes
else
{
//Buscar colision con eje con Picking
pickingRay.updateRay();
gizmoMesh.selectAxisByPicking(pickingRay.Ray);
}
break;
case State.Dragging:
//Mover
if (input.buttonDown(TgcD3dInput.MouseButtons.BUTTON_LEFT))
{
pickingRay.updateRay();
Vector3 endMouseP3d = initMouseP3d;
//Solo se mueve un eje
Vector3 currentMove = new Vector3(0, 0, 0);
if (gizmoMesh.isSingleAxis(gizmoMesh.SelectedAxis))
{
//Desplazamiento en eje X
if (gizmoMesh.SelectedAxis == TranslateGizmoMesh.Axis.X)
{
endMouseP3d = editablePoly.Control.Grid.getPickingX(pickingRay.Ray, gizmoMesh.GizmoCenter);
currentMove.X = endMouseP3d.X - initMouseP3d.X;
}
//Desplazamiento en eje Y
else if (gizmoMesh.SelectedAxis == TranslateGizmoMesh.Axis.Y)
{
endMouseP3d = editablePoly.Control.Grid.getPickingY(pickingRay.Ray, gizmoMesh.GizmoCenter);
currentMove.Y = endMouseP3d.Y - initMouseP3d.Y;
}
//Desplazamiento en eje Z
else if (gizmoMesh.SelectedAxis == TranslateGizmoMesh.Axis.Z)
{
endMouseP3d = editablePoly.Control.Grid.getPickingZ(pickingRay.Ray, gizmoMesh.GizmoCenter);
currentMove.Z = endMouseP3d.Z - initMouseP3d.Z;
}
}
//Desplazamiento en dos ejes
else
{
//Plano XZ
if (gizmoMesh.SelectedAxis == TranslateGizmoMesh.Axis.XZ)
{
endMouseP3d = editablePoly.Control.Grid.getPickingXZ(pickingRay.Ray, gizmoMesh.GizmoCenter);
currentMove.X = endMouseP3d.X - initMouseP3d.X;
currentMove.Z = endMouseP3d.Z - initMouseP3d.Z;
}
//Plano XY
else if (gizmoMesh.SelectedAxis == TranslateGizmoMesh.Axis.XY)
{
endMouseP3d = editablePoly.Control.Grid.getPickingXY(pickingRay.Ray, gizmoMesh.GizmoCenter);
currentMove.X = endMouseP3d.X - initMouseP3d.X;
currentMove.Y = endMouseP3d.Y - initMouseP3d.Y;
}
//Plano YZ
else if (gizmoMesh.SelectedAxis == TranslateGizmoMesh.Axis.YZ)
{
endMouseP3d = editablePoly.Control.Grid.getPickingYZ(pickingRay.Ray, gizmoMesh.GizmoCenter);
currentMove.Y = endMouseP3d.Y - initMouseP3d.Y;
currentMove.Z = endMouseP3d.Z - initMouseP3d.Z;
}
}
//Actualizar pos del mouse 3D para proximo cuadro
initMouseP3d = endMouseP3d;
//Ajustar currentMove con Snap to grid
if (editablePoly.Control.SnapToGridEnabled)
{
snapMovementToGrid(ref currentMove.X, ref acumMovement.X, editablePoly.Control.SnapToGridCellSize);
snapMovementToGrid(ref currentMove.Y, ref acumMovement.Y, editablePoly.Control.SnapToGridCellSize);
snapMovementToGrid(ref currentMove.Z, ref acumMovement.Z, editablePoly.Control.SnapToGridCellSize);
}
if (currentMove.LengthSq() > 0.1f)
{
//Mover objetos
foreach (EditPolyPrimitive p in editablePoly.SelectionList)
{
p.move(currentMove);
}
//Mover ejes
gizmoMesh.moveGizmo(currentMove);
//Actualizar mesh
editablePoly.setDirtyValues(false);
//Actualizar datos de modify
//Control.updateModifyPanel();
}
}
//Soltar movimiento
else if (input.buttonUp(TgcD3dInput.MouseButtons.BUTTON_LEFT))
{
currentState = State.Init;
gizmoMesh.unSelect();
}
break;
}
//Ajustar tamaño de ejes
gizmoMesh.setCenter(gizmoMesh.GizmoCenter, editablePoly.Control.Camera);
}
/// <summary>
/// Detección de colisiones, filtrando los obstaculos que se encuentran dentro del radio de movimiento
/// </summary>
private void collideWithWorld(TgcBoundingSphere characterSphere, Vector3 movementVector, List<Collider> colliders, bool sliding, float slidingMinY)
{
//Ver si la distancia a recorrer es para tener en cuenta
float distanceToTravelSq = movementVector.LengthSq();
if (distanceToTravelSq < EPSILON)
{
return;
}
//Dejar solo los obstáculos que están dentro del radio de movimiento de la esfera
Vector3 halfMovementVec = Vector3.Multiply(movementVector, 0.5f);
movementSphere.setValues(
characterSphere.Center + halfMovementVec,
halfMovementVec.Length() + characterSphere.Radius
);
objetosCandidatos.Clear();
foreach (Collider collider in colliders)
{
if (collider.Enable && TgcCollisionUtils.testSphereSphere(movementSphere, collider.BoundingSphere))
{
objetosCandidatos.Add(collider);
}
}
//Detectar colisiones y deplazar con sliding
doCollideWithWorld(characterSphere, movementVector, objetosCandidatos, 0, movementSphere, sliding, slidingMinY);
}
public Vector3 getMaxPos()
{
Vector3 max = new Vector3();
foreach (Block block in brickBlocks)
{
if (block.getPos().LengthSq() > max.LengthSq())
{
max = block.getPos();
}
}
return max;
}
/// <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>
/// Detección de colisiones, filtrando los obstaculos que se encuentran dentro del radio de movimiento
/// </summary>
private void collideWithWorld(TgcBoundingSphere characterSphere, Vector3 movementVector, List<TgcBoundingBox> obstaculos)
{
if (movementVector.LengthSq() < EPSILON)
{
return;
}
Vector3 lastCenterSafePosition = characterSphere.Center;
//Dejar solo los obstáculos que están dentro del radio de movimiento de la esfera
Vector3 halfMovementVec = Vector3.Multiply(movementVector, 0.5f);
TgcBoundingSphere testSphere = new TgcBoundingSphere(
characterSphere.Center + halfMovementVec,
halfMovementVec.Length() + characterSphere.Radius
);
objetosCandidatos.Clear();
foreach (TgcBoundingBox obstaculo in obstaculos)
{
if (TgcCollisionUtils.testSphereAABB(testSphere, obstaculo))
{
objetosCandidatos.Add(obstaculo);
}
}
//Detectar colisiones y deplazar con sliding
doCollideWithWorld(characterSphere, movementVector, objetosCandidatos, 0);
//Manejo de error. No deberiamos colisionar con nadie si todo salio bien
foreach (TgcBoundingBox obstaculo in objetosCandidatos)
{
if (TgcCollisionUtils.testSphereAABB(characterSphere, obstaculo))
{
//Hubo un error, volver a la posición original
characterSphere.setCenter(lastCenterSafePosition);
return;
}
}
}