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