/// <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);
        }
Пример #4
0
 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;
                }
            }
        }