Beispiel #1
0
        ///bilateral constraint between two dynamic objects
        ///positive distance = separation, negative distance = penetration
        public static void ResolveSingleBilateral(RigidBody body1, ref Vector3 pos1,
                              RigidBody body2, ref Vector3 pos2,
                              float distance, ref Vector3 normal, ref float impulse, float timeStep)
        {
	        float normalLenSqr = normal.LengthSquared();
	        Debug.Assert(System.Math.Abs(normalLenSqr) < 1.1f);
	        if (normalLenSqr > 1.1f)
	        {
		        impulse = 0f;
		        return;
	        }
	        Vector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition(); 
	        Vector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition();
	        //this jacobian entry could be re-used for all iterations
        	
	        Vector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
	        Vector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2);
	        Vector3 vel = vel1 - vel2;

            Matrix m1 = MathUtil.TransposeBasis(body1.GetCenterOfMassTransform());
            Matrix m2 = MathUtil.TransposeBasis(body2.GetCenterOfMassTransform());


            JacobianEntry jac = new JacobianEntry(m1,m2,rel_pos1,rel_pos2,normal,
                body1.GetInvInertiaDiagLocal(),body1.GetInvMass(),
		        body2.GetInvInertiaDiagLocal(),body2.GetInvMass());

	        float jacDiagAB = jac.GetDiagonal();
	        float jacDiagABInv = 1f / jacDiagAB;

            
            float rel_vel = jac.GetRelativeVelocity(
                body1.GetLinearVelocity(),Vector3.TransformNormal(body1.GetAngularVelocity(),m1),
                body2.GetLinearVelocity(),Vector3.TransformNormal(body2.GetAngularVelocity(),m2));
	        float a = jacDiagABInv;

            rel_vel = Vector3.Dot(normal,vel);
        	
	        //todo: move this into proper structure
	        float contactDamping = 0.2f;

            if(ONLY_USE_LINEAR_MASS)
            {
	            float massTerm = 1f / (body1.GetInvMass() + body2.GetInvMass());
	            impulse = - contactDamping * rel_vel * massTerm;
            }
            else	
            {
	            float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
	            impulse = velocityImpulse;
            }
        }
Beispiel #2
0
 public Rectangle(Vector3 p, Vector3 _a, Vector3 _b, Vector3 _normal, string name)
     : base(name)
 {
     p0 = p;
     a = _a;
     b = _b;
     a_len_squared = a.LengthSquared();
     b_len_squared = b.LengthSquared();
     area = a.Length() * b.Length();
     inv_area = 1.0f / area;
     normal = _normal;
     Shadows = false;
 }
Beispiel #3
0
 public bool AddSupportPoint(ref Vector3 newPoint)
 {
     int index = (BitsToIndices[this.simplexBits ^ 15] & 7) - 1;
     this.y[index] = newPoint;
     this.yLengthSq[index] = newPoint.LengthSquared();
     for (int i = BitsToIndices[this.simplexBits]; i != 0; i = i >> 3)
     {
         int num2 = (i & 7) - 1;
         Vector3 vector = this.y[num2] - newPoint;
         this.edges[num2][index] = vector;
         this.edges[index][num2] = Vector3.One - vector;
         this.edgeLengthSq[index][num2] = this.edgeLengthSq[num2][index] = vector.LengthSquared();
     }
     this.UpdateDeterminant(index);
     return this.UpdateSimplex(index);
 }
Beispiel #4
0
        static TestCollidableBEPU[] GetRandomLeavesBEPU(int leafCount, BoundingBox bounds, Vector3 minimumSize, Vector3 maximumSize, float sizePower, VelocityDescription velocityDescription)
        {
            var leaves = new TestCollidableBEPU[leafCount];
            Random random = new Random(5);

            var range = bounds.Max - bounds.Min;
            var sizeRange = maximumSize - minimumSize;
            for (int i = 0; i < leafCount; ++i)
            {
                var halfLeafSize = 0.5f * (minimumSize + new Vector3((float)Math.Pow(random.NextDouble(), sizePower), (float)Math.Pow(random.NextDouble(), sizePower), (float)Math.Pow(random.NextDouble(), sizePower)) * sizeRange);
                var position = bounds.Min + new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) * range;

                leaves[i] = new TestCollidableBEPU();
                leaves[i].Position = new BEPUutilities.Vector3(position.X, position.Y, position.Z);
                leaves[i].HalfSize = new BEPUutilities.Vector3(halfLeafSize.X, halfLeafSize.Y, halfLeafSize.Z);
                leaves[i].UpdateBoundingBox();
            }

            for (int i = 0; i < leaves.Length * velocityDescription.PortionOfMovingLeaves; ++i)
            {
                var speed = (float)(velocityDescription.MinVelocity + (velocityDescription.MaxVelocity - velocityDescription.MinVelocity) * Math.Pow(random.NextDouble(), velocityDescription.VelocityDistributionPower));
                var direction = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) * 2 - Vector3.One;
                var lengthSquared = direction.LengthSquared();
                if (lengthSquared < 1e-9f)
                {
                    direction = new Vector3(0, 1, 0);
                }
                else
                {
                    direction /= (float)Math.Sqrt(lengthSquared);
                }
                var velocity = speed * direction;
                leaves[i].Velocity = new BEPUutilities.Vector3(velocity.X, velocity.Y, velocity.Z);
            }
            return leaves;

        }
Beispiel #5
0
	    public static void CalculateDiffAxisAngle(ref Matrix transform0,ref Matrix transform1,ref Vector3 axis,ref float angle)
	    {
            //Matrix dmat = GetRotateMatrix(ref transform1) * Matrix.Invert(GetRotateMatrix(ref transform0));
            Matrix dmat = MathUtil.BulletMatrixMultiplyBasis(transform1, Matrix.Invert(transform0));
		    Quaternion dorn = Quaternion.Identity;
		    GetRotation(ref dmat,out dorn);

		    ///floating point inaccuracy can lead to w component > 1..., which breaks 
		    dorn.Normalize();
    		
		    angle = MathUtil.QuatAngle(ref dorn);
            
		    axis = new Vector3(dorn.X,dorn.Y,dorn.Z);
            //axis[3] = float(0.);
		    //check for axis length
		    float len = axis.LengthSquared();
            if (len < MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
            {
                axis = Vector3.Right;
            }
            else
            {
                axis.Normalize();
            }
	    }
Beispiel #6
0
        private bool DoDeepContact(out ContactData contact)
        {
            #region Informed search
            if (previousState == CollisionState.Separated) //If it was shallow before, then its closest points will be used to find the normal.
            {
                //It's overlapping! Find the relative velocity at the point relative to the two objects.  The point is still in local space!
                //Vector3 velocityA;
                //Vector3.Cross(ref contact.Position, ref collidableA.entity.angularVelocity, out velocityA);
                //Vector3.Add(ref velocityA, ref collidableA.entity.linearVelocity, out velocityA);
                //Vector3 velocityB;
                //Vector3.Subtract(ref contact.Position, ref localTransformB.Position, out velocityB);
                //Vector3.Cross(ref velocityB, ref collidableB.entity.angularVelocity, out velocityB);
                //Vector3.Add(ref velocityB, ref collidableB.entity.linearVelocity, out velocityB);
                ////The velocity is negated because the direction so point backwards along the velocity.
                //Vector3.Subtract(ref velocityA, ref velocityB, out localDirection);

                //The above takes into account angular velocity, but linear velocity alone is a lot more stable and does the job just fine.
                if (collidableA.entity != null && collidableB.entity != null)
                {
                    Vector3.Subtract(ref collidableA.entity.linearVelocity, ref collidableB.entity.linearVelocity, out localDirection);
                }
                else
                {
                    localDirection = localSeparatingAxis;
                }

                if (localDirection.LengthSquared() < Toolbox.Epsilon)
                {
                    localDirection = Vector3.Up;
                }
            }
            if (MPRToolbox.GetContact(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref localDirection, out contact))
            {
                if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin)
                {
                    state = CollisionState.ShallowContact;
                }
                return(true);
            }
            //This is rare, but could happen.
            state = CollisionState.Separated;
            return(false);

            //if (MPRTesting.GetLocalOverlapPosition(collidableA.Shape, collidableB.Shape, ref localTransformB, out contact.Position))
            //{


            //    //First, try to use the heuristically found direction.  This comes from either the GJK shallow contact separating axis or from the relative velocity.
            //    Vector3 rayCastDirection;
            //    float lengthSquared = localDirection.LengthSquared();
            //    if (lengthSquared > Toolbox.Epsilon)
            //    {
            //        Vector3.Divide(ref localDirection, (float)Math.Sqrt(lengthSquared), out rayCastDirection);// (Vector3.Normalize(localDirection) + Vector3.Normalize(collidableB.worldTransform.Position - collidableA.worldTransform.Position)) / 2;
            //        MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out contact.PenetrationDepth, out contact.Normal);
            //    }
            //    else
            //    {
            //        contact.PenetrationDepth = float.MaxValue;
            //        contact.Normal = Toolbox.UpVector;
            //    }
            //    //Try the offset between the origins as a second option.  Sometimes this is a better choice than the relative velocity.
            //    //TODO: Could use the position-finding MPR iteration to find the A-B direction hit by continuing even after the origin has been found (optimization).
            //    Vector3 normalCandidate;
            //    float depthCandidate;
            //    lengthSquared = localTransformB.Position.LengthSquared();
            //    if (lengthSquared > Toolbox.Epsilon)
            //    {
            //        Vector3.Divide(ref localTransformB.Position, (float)Math.Sqrt(lengthSquared), out rayCastDirection);
            //        MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out depthCandidate, out normalCandidate);
            //        if (depthCandidate < contact.PenetrationDepth)
            //        {
            //            contact.Normal = normalCandidate;
            //        }
            //    }


            //    //Correct the penetration depth.
            //    MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out contact.PenetrationDepth, out rayCastDirection);


            //    ////The local casting can optionally continue.  Eventually, it will converge to the local minimum.
            //    //while (true)
            //    //{
            //    //    MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out depthCandidate, out normalCandidate);
            //    //    if (contact.PenetrationDepth - depthCandidate <= Toolbox.BigEpsilon)
            //    //        break;

            //    //    contact.PenetrationDepth = depthCandidate;
            //    //    contact.Normal = normalCandidate;
            //    //}

            //    contact.Id = -1;
            //    //we're still in local space! transform it all back.
            //    Matrix3X3 orientation;
            //    Matrix3X3.CreateFromQuaternion(ref collidableA.worldTransform.Orientation, out orientation);
            //    Matrix3X3.Transform(ref contact.Normal, ref orientation, out contact.Normal);
            //    //Vector3.Negate(ref contact.Normal, out contact.Normal);
            //    Matrix3X3.Transform(ref contact.Position, ref orientation, out contact.Position);
            //    Vector3.Add(ref contact.Position, ref collidableA.worldTransform.Position, out contact.Position);
            //    if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin)
            //        state = CollisionState.ShallowContact;
            //    return true;
            //}

            ////This is rare, but could happen.
            //state = CollisionState.Separated;
            //contact = new ContactData();
            //return false;
            #endregion

            #region Testing
            //RigidTransform localTransformB;
            //MinkowskiToolbox.GetLocalTransform(ref collidableA.worldTransform, ref collidableB.worldTransform, out localTransformB);
            //contact.Id = -1;
            //if (MPRTesting.GetLocalOverlapPosition(collidableA.Shape, collidableB.Shape, ref localTransformB, out contact.Position))
            //{
            //    Vector3 rayCastDirection = localTransformB.Position;
            //    MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out contact.PenetrationDepth, out contact.Normal);
            //    MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out contact.PenetrationDepth, out rayCastDirection);
            //    RigidTransform.Transform(ref contact.Position, ref collidableA.worldTransform, out contact.Position);
            //    Vector3.Transform(ref contact.Normal, ref collidableA.worldTransform.Orientation, out contact.Normal);
            //    return true;
            //}
            //contact.Normal = new Vector3();
            //contact.PenetrationDepth = 0;
            //return false;
            #endregion

            #region v0.15.2 and before
            //if (MPRToolbox.AreObjectsColliding(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, out contact))
            //{
            //    if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin)
            //        state = CollisionState.ShallowContact; //If it's emerged from the deep contact, we can go back to using the preferred GJK method.
            //    return true;
            //}
            ////This is rare, but could happen.
            //state = CollisionState.Separated;
            //return false;
            #endregion
        }
        ///<summary>
        /// Performs the frame's configuration step.
        ///</summary>
        ///<param name="dt">Timestep duration.</param>
        public override void Update(float dt)
        {
            //Transform point into world space.
            Matrix3x3.Transform(ref localPoint, ref entity.orientationMatrix, out r);
            Vector3.Add(ref r, ref entity.position, out worldPoint);

            float updateRate = 1 / dt;

            if (settings.mode == MotorMode.Servomechanism)
            {
                Vector3.Subtract(ref settings.servo.goal, ref worldPoint, out error);
                float separationDistance = error.Length();
                if (separationDistance > Toolbox.BigEpsilon)
                {
                    float errorReduction;
                    settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, updateRate, out errorReduction, out usedSoftness);

                    //The rate of correction can be based on a constant correction velocity as well as a 'spring like' correction velocity.
                    //The constant correction velocity could overshoot the destination, so clamp it.
                    float correctionSpeed = MathHelper.Min(settings.servo.baseCorrectiveSpeed, separationDistance * updateRate) +
                                            separationDistance * errorReduction;

                    Vector3.Multiply(ref error, correctionSpeed / separationDistance, out biasVelocity);
                    //Ensure that the corrective velocity doesn't exceed the max.
                    float length = biasVelocity.LengthSquared();
                    if (length > settings.servo.maxCorrectiveVelocitySquared)
                    {
                        float multiplier = settings.servo.maxCorrectiveVelocity / (float)Math.Sqrt(length);
                        biasVelocity.X *= multiplier;
                        biasVelocity.Y *= multiplier;
                        biasVelocity.Z *= multiplier;
                    }
                }
                else
                {
                    //Wouldn't want to use a bias from an earlier frame.
                    biasVelocity = new Vector3();
                }
            }
            else
            {
                usedSoftness = settings.velocityMotor.softness * updateRate;
                biasVelocity = settings.velocityMotor.goalVelocity;
                error        = Vector3.Zero;
            }

            //Compute the maximum force that can be applied this frame.
            ComputeMaxForces(settings.maximumForce, dt);

            //COMPUTE EFFECTIVE MASS MATRIX
            //Transforms a change in velocity to a change in momentum when multiplied.
            Matrix3x3 linearComponent;

            Matrix3x3.CreateScale(entity.inverseMass, out linearComponent);
            Matrix3x3 rACrossProduct;

            Matrix3x3.CreateCrossProduct(ref r, out rACrossProduct);
            Matrix3x3 angularComponentA;

            Matrix3x3.Multiply(ref rACrossProduct, ref entity.inertiaTensorInverse, out angularComponentA);
            Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
            Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out effectiveMassMatrix);

            effectiveMassMatrix.M11 += usedSoftness;
            effectiveMassMatrix.M22 += usedSoftness;
            effectiveMassMatrix.M33 += usedSoftness;

            Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
        }
Beispiel #8
0
        internal static Vector3 SanitizeNormal(this Vector3 normal)
        {
            var isn = normal._IsFinite() && normal.LengthSquared() > 0;

            return(isn ? Vector3.Normalize(normal) : Vector3.UnitZ);
        }
Beispiel #9
0
        /// <summary>
        /// Computes per-frame information necessary for the constraint.
        /// </summary>
        /// <param name="dt">Time step duration.</param>
        public override void Update(float dt)
        {
            bool isTryingToMove = movementDirection3d.LengthSquared() > 0;

            if (!isTryingToMove)
            {
                TargetSpeed = 0;
            }

            maxForce = MaximumForce * dt;


            //Compute the jacobians.  This is basically a PointOnLineJoint with motorized degrees of freedom.
            Vector3 downDirection = characterBody.orientationMatrix.Down;

            if (MovementMode != MovementMode.Floating)
            {
                //Compute the linear jacobians first.
                if (isTryingToMove)
                {
                    Vector3 velocityDirection;
                    Vector3 offVelocityDirection;
                    //Project the movement direction onto the support plane defined by the support normal.
                    //This projection is NOT along the support normal to the plane; that would cause the character to veer off course when moving on slopes.
                    //Instead, project along the sweep direction to the plane.
                    //For a 6DOF character controller, the lineStart would be different; it must be perpendicular to the local up.
                    Vector3 lineStart = movementDirection3d;

                    Vector3 lineEnd;
                    Vector3.Add(ref lineStart, ref downDirection, out lineEnd);
                    Plane plane = new Plane(supportData.Normal, 0);
                    float t;
                    //This method can return false when the line is parallel to the plane, but previous tests and the slope limit guarantee that it won't happen.
                    Toolbox.GetLinePlaneIntersection(ref lineStart, ref lineEnd, ref plane, out t, out velocityDirection);

                    //The origin->intersection line direction defines the horizontal velocity direction in 3d space.
                    velocityDirection.Normalize();


                    //The normal and velocity direction are perpendicular and normal, so the off velocity direction doesn't need to be normalized.
                    Vector3.Cross(ref velocityDirection, ref supportData.Normal, out offVelocityDirection);

                    linearJacobianA1 = velocityDirection;
                    linearJacobianA2 = offVelocityDirection;
                    linearJacobianB1 = -velocityDirection;
                    linearJacobianB2 = -offVelocityDirection;
                }
                else
                {
                    //If the character isn't trying to move, then the velocity directions are not well defined.
                    //Instead, pick two arbitrary vectors on the support plane.
                    //First guess will be based on the previous jacobian.
                    //Project the old linear jacobian onto the support normal plane.
                    float dot;
                    Vector3.Dot(ref linearJacobianA1, ref supportData.Normal, out dot);
                    Vector3 toRemove;
                    Vector3.Multiply(ref supportData.Normal, dot, out toRemove);
                    Vector3.Subtract(ref linearJacobianA1, ref toRemove, out linearJacobianA1);

                    //Vector3.Cross(ref linearJacobianA2, ref supportData.Normal, out linearJacobianA1);
                    float length = linearJacobianA1.LengthSquared();
                    if (length < Toolbox.Epsilon)
                    {
                        //First guess failed.  Try the right vector.
                        Vector3.Cross(ref Toolbox.RightVector, ref supportData.Normal, out linearJacobianA1);
                        length = linearJacobianA1.LengthSquared();
                        if (length < Toolbox.Epsilon)
                        {
                            //Okay that failed too! try the forward vector.
                            Vector3.Cross(ref Toolbox.ForwardVector, ref supportData.Normal, out linearJacobianA1);
                            length = linearJacobianA1.LengthSquared();
                            //Unless something really weird is happening, we do not need to test any more axes.
                        }
                    }
                    Vector3.Divide(ref linearJacobianA1, (float)Math.Sqrt(length), out linearJacobianA1);
                    //Pick another perpendicular vector.  Don't need to normalize it since the normal and A1 are already normalized and perpendicular.
                    Vector3.Cross(ref linearJacobianA1, ref supportData.Normal, out linearJacobianA2);

                    //B's linear jacobians are just -A's.
                    linearJacobianB1 = -linearJacobianA1;
                    linearJacobianB2 = -linearJacobianA2;
                }

                if (supportEntity != null)
                {
                    //Compute the angular jacobians.
                    Vector3 supportToContact = supportData.Position - supportEntity.Position;
                    //Since we treat the character to have infinite inertia, we're only concerned with the support's angular jacobians.
                    //Note the order of the cross product- it is reversed to negate the result.
                    Vector3.Cross(ref linearJacobianA1, ref supportToContact, out angularJacobianB1);
                    Vector3.Cross(ref linearJacobianA2, ref supportToContact, out angularJacobianB2);
                }
                else
                {
                    //If we're not standing on an entity, there are no angular jacobians.
                    angularJacobianB1 = new Vector3();
                    angularJacobianB2 = new Vector3();
                }
            }
            else
            {
                //If the character is floating, then the jacobians are simply the 3d movement direction and the perpendicular direction on the character's horizontal plane.
                linearJacobianA1 = movementDirection3d;
                linearJacobianA2 = Vector3.Cross(linearJacobianA1, characterBody.orientationMatrix.Down);
            }


            //Compute the target velocity (in constraint space) for this frame.  The hard work has already been done.
            targetVelocity.X = TargetSpeed;
            targetVelocity.Y = 0;

            //Compute the effective mass matrix.
            if (supportEntity != null && supportEntity.IsDynamic)
            {
                float   m11, m22, m1221 = 0;
                float   inverseMass;
                Vector3 intermediate;

                inverseMass = characterBody.InverseMass;
                m11         = inverseMass;
                m22         = inverseMass;


                //Scale the inertia and mass of the support.  This will make the solver view the object as 'heavier' with respect to horizontal motion.
                Matrix3x3 inertiaInverse = supportEntity.InertiaTensorInverse;
                Matrix3x3.Multiply(ref inertiaInverse, supportForceFactor, out inertiaInverse);
                float extra;
                inverseMass = supportForceFactor * supportEntity.InverseMass;
                Matrix3x3.Transform(ref angularJacobianB1, ref inertiaInverse, out intermediate);
                Vector3.Dot(ref intermediate, ref angularJacobianB1, out extra);
                m11 += inverseMass + extra;
                Vector3.Dot(ref intermediate, ref angularJacobianB2, out extra);
                m1221 += extra;
                Matrix3x3.Transform(ref angularJacobianB2, ref inertiaInverse, out intermediate);
                Vector3.Dot(ref intermediate, ref angularJacobianB2, out extra);
                m22 += inverseMass + extra;


                massMatrix.M11 = m11;
                massMatrix.M12 = m1221;
                massMatrix.M21 = m1221;
                massMatrix.M22 = m22;
                Matrix2x2.Invert(ref massMatrix, out massMatrix);
            }
            else
            {
                //If we're not standing on a dynamic entity, then the mass matrix is defined entirely by the character.
                Matrix2x2.CreateScale(characterBody.Mass, out massMatrix);
            }

            //If we're trying to stand still on an object that's moving, use a position correction term to keep the character
            //from drifting due to accelerations.
            //First thing to do is to check to see if we're moving into a traction/trying to stand still state from a
            //non-traction || trying to move state.  Either that, or we've switched supports and need to update the offset.
            if (supportEntity != null && ((wasTryingToMove && !isTryingToMove) || (!hadTraction && supportFinder.HasTraction) || supportEntity != previousSupportEntity))
            {
                //We're transitioning into a new 'use position correction' state.
                //Force a recomputation of the local offset.
                //The time since transition is used as a flag.
                timeSinceTransition = 0;
            }

            //The state is now up to date.  Compute an error and velocity bias, if needed.
            if (!isTryingToMove && MovementMode == MovementMode.Traction && supportEntity != null)
            {
                var distanceToBottomOfCharacter = supportFinder.BottomDistance;

                if (timeSinceTransition >= 0 && timeSinceTransition < timeUntilPositionAnchor)
                {
                    timeSinceTransition += dt;
                }
                if (timeSinceTransition >= timeUntilPositionAnchor)
                {
                    Vector3.Multiply(ref downDirection, distanceToBottomOfCharacter, out positionLocalOffset);
                    positionLocalOffset = (positionLocalOffset + characterBody.Position) - supportEntity.Position;
                    positionLocalOffset = Matrix3x3.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
                    timeSinceTransition = -1; //Negative 1 means that the offset has been computed.
                }
                if (timeSinceTransition < 0)
                {
                    Vector3 targetPosition;
                    Vector3.Multiply(ref downDirection, distanceToBottomOfCharacter, out targetPosition);
                    targetPosition += characterBody.Position;
                    Vector3 worldSupportLocation = Matrix3x3.Transform(positionLocalOffset, supportEntity.OrientationMatrix) + supportEntity.Position;
                    Vector3 error;
                    Vector3.Subtract(ref targetPosition, ref worldSupportLocation, out error);
                    //If the error is too large, then recompute the offset.  We don't want the character rubber banding around.
                    if (error.LengthSquared() > PositionAnchorDistanceThreshold * PositionAnchorDistanceThreshold)
                    {
                        Vector3.Multiply(ref downDirection, distanceToBottomOfCharacter, out positionLocalOffset);
                        positionLocalOffset    = (positionLocalOffset + characterBody.Position) - supportEntity.Position;
                        positionLocalOffset    = Matrix3x3.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
                        positionCorrectionBias = new Vector2();
                    }
                    else
                    {
                        //The error in world space is now available.  We can't use this error to directly create a velocity bias, though.
                        //It needs to be transformed into constraint space where the constraint operates.
                        //Use the jacobians!
                        Vector3.Dot(ref error, ref linearJacobianA1, out positionCorrectionBias.X);
                        Vector3.Dot(ref error, ref linearJacobianA2, out positionCorrectionBias.Y);
                        //Scale the error so that a portion of the error is resolved each frame.
                        Vector2.Multiply(ref positionCorrectionBias, .2f / dt, out positionCorrectionBias);
                    }
                }
            }
            else
            {
                timeSinceTransition    = 0;
                positionCorrectionBias = new Vector2();
            }

            wasTryingToMove       = isTryingToMove;
            hadTraction           = supportFinder.HasTraction;
            previousSupportEntity = supportEntity;
        }
Beispiel #10
0
        /// <summary>
        /// Calculates necessary information for velocity solving.
        /// Called by preStep(float dt)
        /// </summary>
        /// <param name="dt">Time in seconds since the last update.</param>
        public override void Update(float dt)
        {
            Matrix3X3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA);
            Matrix3X3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB);


            float errorReductionParameter;

            springSettings.ComputeErrorReductionAndSoftness(dt, out errorReductionParameter, out softness);

            //Mass Matrix
            Matrix3X3 k;
            Matrix3X3 linearComponent;

            Matrix3X3.CreateCrossProduct(ref worldOffsetA, out rACrossProduct);
            Matrix3X3.CreateCrossProduct(ref worldOffsetB, out rBCrossProduct);
            if (connectionA.isDynamic && connectionB.isDynamic)
            {
                Matrix3X3.CreateScale(connectionA.inverseMass + connectionB.inverseMass, out linearComponent);
                Matrix3X3 angularComponentA, angularComponentB;
                Matrix3X3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA);
                Matrix3X3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB);
                Matrix3X3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
                Matrix3X3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB);
                Matrix3X3.Subtract(ref linearComponent, ref angularComponentA, out k);
                Matrix3X3.Subtract(ref k, ref angularComponentB, out k);
            }
            else if (connectionA.isDynamic && !connectionB.isDynamic)
            {
                Matrix3X3.CreateScale(connectionA.inverseMass, out linearComponent);
                Matrix3X3 angularComponentA;
                Matrix3X3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA);
                Matrix3X3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
                Matrix3X3.Subtract(ref linearComponent, ref angularComponentA, out k);
            }
            else if (!connectionA.isDynamic && connectionB.isDynamic)
            {
                Matrix3X3.CreateScale(connectionB.inverseMass, out linearComponent);
                Matrix3X3 angularComponentB;
                Matrix3X3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB);
                Matrix3X3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB);
                Matrix3X3.Subtract(ref linearComponent, ref angularComponentB, out k);
            }
            else
            {
                throw new InvalidOperationException("Cannot constrain two kinematic bodies.");
            }
            k.M11 += softness;
            k.M22 += softness;
            k.M33 += softness;
            Matrix3X3.Invert(ref k, out massMatrix);

            Vector3.Add(ref connectionB.position, ref worldOffsetB, out error);
            Vector3.Subtract(ref error, ref connectionA.position, out error);
            Vector3.Subtract(ref error, ref worldOffsetA, out error);


            Vector3.Multiply(ref error, -errorReductionParameter, out biasVelocity);

            //Ensure that the corrective velocity doesn't exceed the max.
            float length = biasVelocity.LengthSquared();

            if (length > maxCorrectiveVelocitySquared)
            {
                float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length);
                biasVelocity.X *= multiplier;
                biasVelocity.Y *= multiplier;
                biasVelocity.Z *= multiplier;
            }
        }
Beispiel #11
0
        //TODO: Consider changing the termination epsilons on these casts.  Epsilon * Modifier is okay, but there might be better options.

        ///<summary>
        /// Tests a ray against a convex shape.
        ///</summary>
        ///<param name="ray">Ray to test against the shape.</param>
        ///<param name="shape">Shape to test.</param>
        ///<param name="shapeTransform">Transform to apply to the shape for the test.</param>
        ///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
        ///<param name="hit">Hit data of the ray cast, if any.</param>
        ///<returns>Whether or not the ray hit the shape.</returns>
        public static bool RayCast(Ray ray, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength,
                                   out RayHit hit)
        {
            //Transform the ray into the object's local space.
            Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
            Quaternion conjugate;

            Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
            Quaternion.Transform(ref ray.Position, ref conjugate, out ray.Position);
            Quaternion.Transform(ref ray.Direction, ref conjugate, out ray.Direction);

            Vector3 extremePointToRayOrigin, extremePoint;

            hit.T        = 0;
            hit.Location = ray.Position;
            hit.Normal   = Toolbox.ZeroVector;
            Vector3 closestOffset = hit.Location;

            RaySimplex simplex = new RaySimplex();

            float vw, closestPointDotDirection;
            int   count = 0;

            //This epsilon has a significant impact on performance and accuracy.  Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
            while (closestOffset.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position))
            {
                if (++count > MaximumGJKIterations)
                {
                    //It's taken too long to find a hit.  Numerical problems are probable; quit.
                    hit = new RayHit();
                    return(false);
                }

                shape.GetLocalExtremePoint(closestOffset, out extremePoint);

                Vector3.Subtract(ref hit.Location, ref extremePoint, out extremePointToRayOrigin);
                Vector3.Dot(ref closestOffset, ref extremePointToRayOrigin, out vw);
                //If the closest offset and the extreme point->ray origin direction point the same way,
                //then we might be able to conservatively advance the point towards the surface.
                if (vw > 0)
                {
                    Vector3.Dot(ref closestOffset, ref ray.Direction, out closestPointDotDirection);
                    if (closestPointDotDirection >= 0)
                    {
                        hit = new RayHit();
                        return(false);
                    }
                    hit.T = hit.T - vw / closestPointDotDirection;
                    if (hit.T > maximumLength)
                    {
                        //If we've gone beyond where the ray can reach, there's obviously no hit.
                        hit = new RayHit();
                        return(false);
                    }
                    //Shift the ray up.
                    Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
                    Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
                    hit.Normal = closestOffset;
                }

                RaySimplex shiftedSimplex;
                simplex.AddNewSimplexPoint(ref extremePoint, ref hit.Location, out shiftedSimplex);

                //Compute the offset from the simplex surface to the origin.
                shiftedSimplex.GetPointClosestToOrigin(ref simplex, out closestOffset);
            }
            //Transform the hit data into world space.
            Quaternion.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
            Quaternion.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
            Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);

            return(true);
        }
Beispiel #12
0
 public static Vector3 Project(Vector3 a, Vector3 b)
 {
     return(b * (Vector3.Dot(a, b) / b.LengthSquared()));
 }
        /// <summary>
        /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities.
        /// </summary>
        /// <param name="connectionA">First entity of the constraint pair.</param>
        /// <param name="connectionB">Second entity of the constraint pair.</param>
        /// <param name="anchor">Point around which both entities rotate.</param>
        /// <param name="hingeAxis">Axis of allowed rotation in world space to be attached to connectionA.  Will be kept perpendicular with the twist axis.</param>
        public SwivelHingeJoint(Entity connectionA, Entity connectionB, Vector3 anchor, Vector3 hingeAxis)
        {
            if (connectionA == null)
            {
                connectionA = TwoEntityConstraint.WorldEntity;
            }
            if (connectionB == null)
            {
                connectionB = TwoEntityConstraint.WorldEntity;
            }
            BallSocketJoint     = new BallSocketJoint(connectionA, connectionB, anchor);
            AngularJoint        = new SwivelHingeAngularJoint(connectionA, connectionB, hingeAxis, -BallSocketJoint.OffsetB);
            HingeLimit          = new RevoluteLimit(connectionA, connectionB);
            HingeMotor          = new RevoluteMotor(connectionA, connectionB, hingeAxis);
            TwistLimit          = new TwistLimit(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB, 0, 0);
            TwistMotor          = new TwistMotor(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB);
            HingeLimit.IsActive = false;
            HingeMotor.IsActive = false;
            TwistLimit.IsActive = false;
            TwistMotor.IsActive = false;

            //Ensure that the base and test direction is perpendicular to the free axis.
            Vector3 baseAxis = anchor - connectionA.position;

            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way.
            {
                baseAxis = connectionB.position - anchor;
            }
            baseAxis -= Vector3.Dot(baseAxis, hingeAxis) * hingeAxis;
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
            {
                //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
                baseAxis = Vector3.Cross(hingeAxis, Vector3.Up);
                if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
                {
                    baseAxis = Vector3.Cross(hingeAxis, Vector3.Right);
                }
            }
            HingeLimit.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix);
            HingeMotor.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix);

            baseAxis  = connectionB.position - anchor;
            baseAxis -= Vector3.Dot(baseAxis, hingeAxis) * hingeAxis;
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
            {
                //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
                baseAxis = Vector3.Cross(hingeAxis, Vector3.Up);
                if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
                {
                    baseAxis = Vector3.Cross(hingeAxis, Vector3.Right);
                }
            }
            HingeLimit.TestAxis = baseAxis;
            HingeMotor.TestAxis = baseAxis;


            Add(BallSocketJoint);
            Add(AngularJoint);
            Add(HingeLimit);
            Add(HingeMotor);
            Add(TwistLimit);
            Add(TwistMotor);
        }
Beispiel #14
0
        private void handle_thrust_control()
        {
            const float DAMPING_CONSTANT = 10.0f;

            foreach (var cur_direction in _thrusters)
            {
                foreach (var cur_thruster_info in cur_direction.Values)
                {
                    if (cur_thruster_info.is_RCS)
                    {
                        cur_thruster_info.override_cleared = false;
                    }
                }
            }

            Matrix inverse_world_rotation = _inverse_world_transform.GetOrientation();

            _local_angular_velocity = Vector3.Transform(_grid.Physics.AngularVelocity, inverse_world_rotation);
            float   manual_rotation_length2 = _manual_rotation.LengthSquared();
            Vector3 desirted_angular_velocity, local_linear_velocity = Vector3.Transform(_grid.Physics.LinearVelocity, inverse_world_rotation);

            if (manual_rotation_length2 <= 0.0001f)
            {
                desirted_angular_velocity = -_local_angular_velocity;
            }
            else
            {
                float   projection_dot_prduct     = Vector3.Dot(_local_angular_velocity, _manual_rotation);
                Vector3 local_velocity_projection = (projection_dot_prduct / manual_rotation_length2) * _manual_rotation,
                        local_velocity_rejection  = _local_angular_velocity - local_velocity_projection;

                if (projection_dot_prduct < 0.0f)
                {
                    local_velocity_projection = Vector3.Zero;
                }
                desirted_angular_velocity = _manual_rotation * 2.0f + local_velocity_projection - local_velocity_rejection;
            }

            _rotation_active = desirted_angular_velocity.LengthSquared() >= 0.0005f;
            if (!_rotation_active || _is_gyro_override_active || autopilot_on)
            {
                apply_thrust_settings(reset_all_thrusters: true);
                return;
            }
            decompose_vector(desirted_angular_velocity, __control_vector);
            decompose_vector(local_linear_velocity, __linear_velocity);
            float min_control = (!linear_dampers_on || local_linear_velocity.LengthSquared() <= 1.0f) ? 0.02f : 0.3f,
                  longitudinal_speed = __linear_velocity[(int)thrust_dir.fore] + __linear_velocity[(int)thrust_dir.aft],
                  lateral_speed      = __linear_velocity[(int)thrust_dir.port] + __linear_velocity[(int)thrust_dir.starboard],
                  vertical_speed     = __linear_velocity[(int)thrust_dir.dorsal] + __linear_velocity[(int)thrust_dir.ventral];
            bool pitch_control_on    = __control_vector[(int)thrust_dir.port] + __control_vector[(int)thrust_dir.starboard] >= min_control,
                 yaw_control_on      = __control_vector[(int)thrust_dir.dorsal] + __control_vector[(int)thrust_dir.ventral] >= min_control,
                 roll_control_on     = __control_vector[(int)thrust_dir.fore] + __control_vector[(int)thrust_dir.aft] >= min_control;

            __rotation_enable[(int)thrust_dir.fore]   = __rotation_enable[(int)thrust_dir.aft] = pitch_control_on || yaw_control_on || longitudinal_speed <= 1.0f;
            __rotation_enable[(int)thrust_dir.port]   = __rotation_enable[(int)thrust_dir.starboard] = yaw_control_on || roll_control_on || lateral_speed <= 1.0f;
            __rotation_enable[(int)thrust_dir.dorsal] = __rotation_enable[(int)thrust_dir.ventral] = pitch_control_on || roll_control_on || vertical_speed <= 1.0f;

            Vector3 local_gravity_force /*, linear_damping*/;

            if (!linear_dampers_on)
            {
                local_gravity_force = /*linear_damping =*/ Vector3.Zero;
            }
            else
            {
                local_gravity_force = Vector3.Transform(_grid.Physics.Gravity * _grid.Physics.Mass, inverse_world_rotation);
                //linear_damping      = (-2.0f * _grid.Physics.Mass) * Vector3.Transform(_grid.Physics.LinearVelocity, inverse_world_rotation);
            }
            //decompose_vector(      linear_damping, __braking_vector );
            decompose_vector(_manual_thrust, __thrust_vector);
            decompose_vector(-local_gravity_force, __counter_gravity);

            /*
             * for (int dir_index = 0; dir_index < 6; ++dir_index)
             * {
             *  if (_lin_force[dir_index] >= 1.0f)
             *      __thrust_vector[dir_index] += __braking_vector[dir_index] / _lin_force[dir_index];
             * }
             */
            for (int dir_index = 0; dir_index < 6; ++dir_index)
            {
                int   opposite_dir = (dir_index < 3) ? (dir_index + 3) : (dir_index - 3);
                float initial_setting;

                if (__thrust_vector[dir_index] < MAX_THRUST_LEVEL && __thrust_vector[opposite_dir] < MAX_THRUST_LEVEL)
                {
                    initial_setting = (_lin_force[dir_index] >= 1.0f) ? (__counter_gravity[dir_index] / _lin_force[dir_index]) : 0.0f;
                }
                else
                {
                    initial_setting = 0.0f;
                }
                foreach (var cur_thruster_info in _thrusters[dir_index].Values)
                {
                    cur_thruster_info.current_setting = initial_setting;
                }
            }
            for (int dir_index = 0; dir_index < 6; ++dir_index)
            {
                int   thruster_dir, opposite_dir;
                float control = DAMPING_CONSTANT * __control_vector[dir_index] * _grid.Physics.Mass;

                for (thruster_dir = 0; thruster_dir < 6; ++thruster_dir)
                {
                    __settings[thruster_dir] = (_max_force[thruster_dir] >= 1.0f) ? (control / _max_force[thruster_dir]) : 0.0f;
                }
                foreach (var cur_thruster_info in _control_sets[dir_index])
                {
                    if (!cur_thruster_info.is_RCS)
                    {
                        continue;
                    }

                    thruster_dir = (int)cur_thruster_info.nozzle_direction;
                    opposite_dir = (thruster_dir < 3) ? (thruster_dir + 3) : (thruster_dir - 3);
                    if (__thrust_vector[thruster_dir] < MAX_THRUST_LEVEL && __thrust_vector[opposite_dir] < MAX_THRUST_LEVEL)
                    {
                        cur_thruster_info.current_setting += __settings[thruster_dir];
                        if (cur_thruster_info.current_setting > 1.0f)
                        {
                            cur_thruster_info.current_setting = 1.0f;
                        }
                    }
                }
            }

            normalise_thrust();
            apply_thrust_settings(reset_all_thrusters: false);
        }
Beispiel #15
0
        private void calculate_and_apply_torque()
        {
            //if (MyAPIGateway.Multiplayer != null && !MyAPIGateway.Multiplayer.IsServer)
            //    return;
            //const float MIN_TORQUE = 10.0f;
            const float MIN_ANGULAR_ACCELERATION = (float)(0.1 * Math.PI / 180.0);

            Vector3 torque = Vector3.Zero, useful_torque, parasitic_torque;
            float   current_strength;

            foreach (var cur_direction in _thrusters)
            {
                foreach (var cur_thruster in cur_direction)
                {
                    //if (cur_thruster.Key.IsWorking)
                    {
                        current_strength = cur_thruster.Key.CurrentStrength;
                        // RC autopilot, yor're cheaty b****rd!
                        if (current_strength > 1.0f)
                        {
                            current_strength = 1.0f;
                        }
                        else if (current_strength < 0.0f)
                        {
                            current_strength = 0.0f;
                        }
                        torque += cur_thruster.Value.max_torque * current_strength;
                    }
                }
            }

            if (!_rotation_active || autopilot_on)
            {
                useful_torque    = Vector3.Zero;
                parasitic_torque = torque;
            }
            else
            {
                /*
                 * Vector3 reference_vector = (_manual_rotation.LengthSquared() > 0.0001f) ? _manual_rotation : _local_angular_velocity;
                 * if (reference_vector.LengthSquared() > 0.0001f)
                 * {
                 *  useful_torque    = (Vector3.Dot(torque, reference_vector) / reference_vector.LengthSquared()) * reference_vector;
                 *  parasitic_torque = torque - useful_torque;
                 * }
                 * else
                 * {
                 *  useful_torque    = torque;
                 *  parasitic_torque = Vector3.Zero;
                 * }
                 */

                float manual_rotation_length2 = _manual_rotation.LengthSquared(), angular_velocity_length2 = _local_angular_velocity.LengthSquared();

                if (manual_rotation_length2 <= 0.0001f)
                {
                    useful_torque = Vector3.Zero;
                }
                else
                {
                    float projection_dot_product = Vector3.Dot(torque, _manual_rotation);

                    useful_torque = (projection_dot_product > 0.0f) ? ((projection_dot_product / manual_rotation_length2) * _manual_rotation) : Vector3.Zero;
                }
                Vector3 leftover_torque = torque - useful_torque;
                if (angular_velocity_length2 > 0.0001f)
                {
                    float projection_dot_product = Vector3.Dot(leftover_torque, _local_angular_velocity);

                    if (projection_dot_product < 0.0f)
                    {
                        useful_torque += (projection_dot_product / angular_velocity_length2) * _local_angular_velocity;
                    }
                }
                parasitic_torque = torque - useful_torque;
            }

            //screen_text("", string.Format("UT = {0:F4} MN*m, PT = {1:F4} MN*m", useful_torque.Length() / 1.0E+6f, parasitic_torque.Length() / 1.0E+6f), 16, controlled_only: true);

            float gyro_limit = _max_gyro_torque;

            /*
             * if (_is_gyro_override_active || _manual_rotation.LengthSquared() <= 0.0001f)
             * {
             *  Vector3 angular_velocity_diff = _local_angular_velocity;
             *
             *  if (_is_gyro_override_active)
             *      angular_velocity_diff -= _gyro_override;
             *  gyro_limit -= angular_velocity_diff.Length() * _spherical_moment_of_inertia;
             * }
             */
            if (gyro_limit < 1.0f)
            {
                gyro_limit = 1.0f;
            }
            if (parasitic_torque.LengthSquared() <= gyro_limit * gyro_limit)
            {
                parasitic_torque = Vector3.Zero;
            }
            else
            {
                parasitic_torque -= Vector3.Normalize(parasitic_torque) * gyro_limit;
            }

            torque = useful_torque + parasitic_torque;
            if (_physics_enable_delay > 0)
            {
                --_physics_enable_delay;
            }
            else if (torque.LengthSquared() > MIN_ANGULAR_ACCELERATION * MIN_ANGULAR_ACCELERATION * _spherical_moment_of_inertia * _spherical_moment_of_inertia)
            {
                torque = Vector3.Transform(torque, _grid.WorldMatrix.GetOrientation());
                _grid.Physics.AddForce(MyPhysicsForceType.APPLY_WORLD_IMPULSE_AND_WORLD_ANGULAR_IMPULSE, Vector3.Zero, null, torque);
            }
        }
Beispiel #16
0
        ///<summary>
        /// Casts a fat (sphere expanded) ray against the shape.
        ///</summary>
        ///<param name="ray">Ray to test against the shape.</param>
        ///<param name="radius">Radius of the ray.</param>
        ///<param name="shape">Shape to test against.</param>
        ///<param name="shapeTransform">Transform to apply to the shape for the test.</param>
        ///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
        ///<param name="hit">Hit data of the sphere cast, if any.</param>
        ///<returns>Whether or not the sphere cast hit the shape.</returns>
        public static bool SphereCast(Ray ray, float radius, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength,
                                      out RayHit hit)
        {
            //Transform the ray into the object's local space.
            Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
            Quaternion conjugate;

            Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
            Quaternion.Transform(ref ray.Position, ref conjugate, out ray.Position);
            Quaternion.Transform(ref ray.Direction, ref conjugate, out ray.Direction);

            Vector3 w, p;

            hit.T        = 0;
            hit.Location = ray.Position;
            hit.Normal   = Toolbox.ZeroVector;
            Vector3 v = hit.Location;

            RaySimplex simplex = new RaySimplex();

            float vw, vdir;
            int   count = 0;

            //This epsilon has a significant impact on performance and accuracy.  Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
            while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position))
            {
                if (++count > MaximumGJKIterations)
                {
                    //It's taken too long to find a hit.  Numerical problems are probable; quit.
                    hit = new RayHit();
                    return(false);
                }

                shape.GetLocalExtremePointWithoutMargin(ref v, out p);
                Vector3 contribution;
                MinkowskiToolbox.ExpandMinkowskiSum(shape.collisionMargin, radius, ref v, out contribution);
                Vector3.Add(ref p, ref contribution, out p);

                Vector3.Subtract(ref hit.Location, ref p, out w);
                Vector3.Dot(ref v, ref w, out vw);
                if (vw > 0)
                {
                    Vector3.Dot(ref v, ref ray.Direction, out vdir);
                    hit.T = hit.T - vw / vdir;
                    if (vdir >= 0)
                    {
                        //We would have to back up!
                        return(false);
                    }
                    if (hit.T > maximumLength)
                    {
                        //If we've gone beyond where the ray can reach, there's obviously no hit.
                        return(false);
                    }
                    //Shift the ray up.
                    Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
                    Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
                    hit.Normal = v;
                }

                RaySimplex shiftedSimplex;
                simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex);

                shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v);
            }
            //Transform the hit data into world space.
            Quaternion.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
            Quaternion.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
            Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);

            return(true);
        }
Beispiel #17
0
        ///<summary>
        /// Sweeps two shapes against another.
        ///</summary>
        ///<param name="shapeA">First shape being swept.</param>
        ///<param name="shapeB">Second shape being swept.</param>
        ///<param name="sweepA">Sweep vector for the first shape.</param>
        ///<param name="sweepB">Sweep vector for the second shape.</param>
        ///<param name="transformA">Transform to apply to the first shape.</param>
        ///<param name="transformB">Transform to apply to the second shape.</param>
        ///<param name="hit">Hit data of the sweep test, if any.</param>
        ///<returns>Whether or not the swept shapes hit each other..</returns>
        public static bool ConvexCast(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 sweepA, ref Vector3 sweepB, ref RigidTransform transformA, ref RigidTransform transformB,
                                      out RayHit hit)
        {
            //Put the velocity into shapeA's local space.
            Vector3 velocityWorld;

            Vector3.Subtract(ref sweepB, ref sweepA, out velocityWorld);
            Quaternion conjugateOrientationA;

            Quaternion.Conjugate(ref transformA.Orientation, out conjugateOrientationA);
            Vector3 rayDirection;

            Quaternion.Transform(ref velocityWorld, ref conjugateOrientationA, out rayDirection);
            //Transform b into a's local space.
            RigidTransform localTransformB;

            Quaternion.Concatenate(ref transformB.Orientation, ref conjugateOrientationA, out localTransformB.Orientation);
            Vector3.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position);
            Quaternion.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position);


            Vector3 w, p;

            hit.T        = 0;
            hit.Location = Vector3.Zero; //The ray starts at the origin.
            hit.Normal   = Toolbox.ZeroVector;
            Vector3 v = hit.Location;

            RaySimplex simplex = new RaySimplex();


            float vw, vdir;
            int   count = 0;

            do
            {
                if (++count > MaximumGJKIterations)
                {
                    //It's taken too long to find a hit.  Numerical problems are probable; quit.
                    hit = new RayHit();
                    return(false);
                }

                MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref v, ref localTransformB, out p);

                Vector3.Subtract(ref hit.Location, ref p, out w);
                Vector3.Dot(ref v, ref w, out vw);
                if (vw > 0)
                {
                    Vector3.Dot(ref v, ref rayDirection, out vdir);
                    if (vdir >= 0)
                    {
                        hit = new RayHit();
                        return(false);
                    }
                    hit.T = hit.T - vw / vdir;
                    if (hit.T > 1)
                    {
                        //If we've gone beyond where the ray can reach, there's obviously no hit.
                        hit = new RayHit();
                        return(false);
                    }
                    //Shift the ray up.
                    Vector3.Multiply(ref rayDirection, hit.T, out hit.Location);
                    //The ray origin is the origin!  Don't need to add any ray position.
                    hit.Normal = v;
                }

                RaySimplex shiftedSimplex;
                simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex);

                shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v);

                //Could measure the progress of the ray.  If it's too little, could early out.
                //Not used by default since it's biased towards precision over performance.
            } while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref Toolbox.ZeroVector));
            //This epsilon has a significant impact on performance and accuracy.  Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
            //Transform the hit data into world space.
            Quaternion.Transform(ref hit.Normal, ref transformA.Orientation, out hit.Normal);
            Vector3.Multiply(ref velocityWorld, hit.T, out hit.Location);
            Vector3.Add(ref hit.Location, ref transformA.Position, out hit.Location);
            return(true);
        }
        ///<summary>
        /// Adds a new point to the simplex.
        ///</summary>
        ///<param name="shapeA">First shape in the pair.</param>
        ///<param name="shapeB">Second shape in the pair.</param>
        ///<param name="iterationCount">Current iteration count.</param>
        ///<param name="closestPoint">Current point on simplex closest to origin.</param>
        ///<returns>Whether or not GJK should exit due to a lack of progression.</returns>
        public bool GetNewSimplexPoint(ConvexShape shapeA, ConvexShape shapeB, int iterationCount, ref Vector3 closestPoint)
        {
            Vector3 negativeDirection;
            Vector3.Negate(ref closestPoint, out negativeDirection);
            Vector3 sa, sb;
            shapeA.GetLocalExtremePointWithoutMargin(ref negativeDirection, out sa);
            shapeB.GetExtremePointWithoutMargin(closestPoint, ref LocalTransformB, out sb);
            Vector3 S;
            Vector3.Subtract(ref sa, ref sb, out S);
            //If S is not further towards the origin along negativeDirection than closestPoint, then we're done.
            float dotS;
            Vector3.Dot(ref S, ref negativeDirection, out dotS); //-P * S
            float distanceToClosest = closestPoint.LengthSquared();

            float progression = dotS + distanceToClosest;
            //It's likely that the system is oscillating between two or more states, usually because of a degenerate simplex.
            //Rather than detect specific problem cases, this approach just lets it run and catches whatever falls through.
            //During oscillation, one of the states is usually just BARELY outside of the numerical tolerance.
            //After a bunch of iterations, the system lets it pick the 'better' one.
            if (iterationCount > GJKToolbox.HighGJKIterations && distanceToClosest - previousDistanceToClosest < DistanceConvergenceEpsilon * errorTolerance)
                return true;
            if (distanceToClosest < previousDistanceToClosest)
                previousDistanceToClosest = distanceToClosest;

            //If "A" is the new point always, then the switch statement can be removed
            //in favor of just pushing three points up.
            switch (State)
            {
                case SimplexState.Point:
                    if (progression <= (errorTolerance = MathHelper.Max(A.LengthSquared(), S.LengthSquared())) * ProgressionEpsilon)
                        return true;

                    State = SimplexState.Segment;
                    B = S;
                    SimplexA.B = sa;
                    SimplexB.B = sb;
                    return false;
                case SimplexState.Segment:
                    if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), S.LengthSquared())) * ProgressionEpsilon)
                        return true;

                    State = SimplexState.Triangle;
                    C = S;
                    SimplexA.C = sa;
                    SimplexB.C = sb;
                    return false;
                case SimplexState.Triangle:
                    if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), MathHelper.Max(C.LengthSquared(), S.LengthSquared()))) * ProgressionEpsilon)
                        return true;

                    State = SimplexState.Tetrahedron;
                    D = S;
                    SimplexA.D = sa;
                    SimplexB.D = sb;
                    return false;
            }
            return false;
        }
Beispiel #19
0
        public static void PathToTarget(NpcInfo npc, AiPathData data, Vector3 pathTargetPos, long deltaTime, bool faceIsMoveFir, AbstractNpcStateLogic logic)
        {
            NpcAiStateInfo info = npc.GetAiStateInfo();

            if (null != data)
            {
                data.UpdateTime += deltaTime;
                ScriptRuntime.Vector3 srcPos = npc.GetMovementStateInfo().GetPosition3D();
                float dir           = npc.GetMovementStateInfo().GetMoveDir();
                bool  findObstacle  = false;
                bool  havePathPoint = data.HavePathPoint;
                if (havePathPoint)//沿路点列表移动的逻辑
                {
                    Vector3 targetPos = data.CurPathPoint;
                    if (!data.IsReached(srcPos))//向指定路点移动(避让移动过程)
                    {
                        float   angle        = Geometry.GetYAngle(new Vector2(srcPos.X, srcPos.Z), new Vector2(targetPos.X, targetPos.Z));
                        Vector3 prefVelocity = (float)npc.GetActualProperty().MoveSpeed *new Vector3((float)Math.Sin(angle), 0, (float)Math.Cos(angle));
                        Vector3 v            = new Vector3(targetPos.X - srcPos.X, 0, targetPos.Z - srcPos.Z);
                        v.Normalize();
                        Vector3 velocity    = npc.SpaceObject.GetVelocity();
                        float   speedSquare = (float)npc.GetActualProperty().MoveSpeed *(float)npc.GetActualProperty().MoveSpeed;
                        long    stTime      = TimeUtility.GetElapsedTimeUs();
                        Vector3 newVelocity = npc.SpatialSystem.ComputeVelocity(npc.SpaceObject, v, (float)deltaTime / 1000, (float)npc.GetActualProperty().MoveSpeed, (float)npc.GetRadius(), data.IsUsingAvoidanceVelocity);
                        findObstacle = !AiLogicUtility.IsWalkable(npc.SpatialSystem.GetCellMapView(npc.AvoidanceRadius), srcPos, newVelocity);
                        long endTime  = TimeUtility.GetElapsedTimeUs();
                        long calcTime = endTime - stTime;
                        if (calcTime > 10000)
                        {
                            LogSystem.Warn("*** pve ComputeVelocity consume {0} us,npc:{1} velocity:{2} newVelocity:{3} deltaTime:{4} speed:{5} pos:{6}", calcTime, npc.GetId(), velocity.ToString(), newVelocity.ToString(), deltaTime, npc.GetActualProperty().MoveSpeed, npc.GetMovementStateInfo().GetPosition3D().ToString());
                        }
                        if (Geometry.DistanceSquare(newVelocity, new Vector3()) <= speedSquare * 0.25f)//避让计算的移动速度变小(说明没有更好的保持原速的选择,停止)
                        {
                            npc.GetMovementStateInfo().IsMoving = false;
                            logic.NotifyNpcMove(npc);
                            data.IsUsingAvoidanceVelocity = false;
                        }
                        else if (findObstacle)//当前移动方向遇到阻挡,停止移动,触发寻路
                        {
                            npc.GetMovementStateInfo().IsMoving = false;
                            logic.NotifyNpcMove(npc);
                            data.IsUsingAvoidanceVelocity = false;
                        }
                        else if (data.UpdateTime > 1000)//避让速度改变每秒一次(表现上更像人类一些)
                        {
                            data.UpdateTime = 0;

                            float newAngle = Geometry.GetYAngle(new Vector2(0, 0), new Vector2(newVelocity.X, newVelocity.Z));
                            npc.GetMovementStateInfo().SetMoveDir(newAngle);
                            if (faceIsMoveFir)
                            {
                                npc.GetMovementStateInfo().SetFaceDir(newAngle);
                            }
                            newVelocity.Normalize();
                            npc.GetMovementStateInfo().TargetPosition = srcPos + newVelocity * Geometry.Distance(srcPos, targetPos);
                            npc.GetMovementStateInfo().IsMoving       = true;
                            logic.NotifyNpcMove(npc);
                            data.IsUsingAvoidanceVelocity = true;
                        }
                        else if (Geometry.DistanceSquare(velocity, newVelocity) > 9.0f) //没有到速度改变周期,但避让方向需要大幅调整
                        {
                            if (Geometry.Dot(newVelocity, prefVelocity) > 0)            //如果是调整为与目标方向一致,则进行调整
                            {
                                float newAngle = Geometry.GetYAngle(new Vector2(0, 0), new Vector2(newVelocity.X, newVelocity.Z));
                                npc.GetMovementStateInfo().SetMoveDir(newAngle);
                                if (faceIsMoveFir)
                                {
                                    npc.GetMovementStateInfo().SetFaceDir(newAngle);
                                }
                                newVelocity.Normalize();
                                npc.GetMovementStateInfo().TargetPosition = srcPos + newVelocity * Geometry.Distance(srcPos, targetPos);
                                npc.GetMovementStateInfo().IsMoving       = true;
                                logic.NotifyNpcMove(npc);
                                data.IsUsingAvoidanceVelocity = true;
                            }
                            else//如果调整为远离目标方向,则停止
                            {
                                npc.GetMovementStateInfo().IsMoving = false;
                                logic.NotifyNpcMove(npc);
                                data.IsUsingAvoidanceVelocity = false;
                            }
                        }
                        else if (!npc.GetMovementStateInfo().IsMoving&& velocity.LengthSquared() > speedSquare * 0.25f)//正常移动过程,继续移动
                        {
                            velocity.Normalize();
                            npc.GetMovementStateInfo().TargetPosition = srcPos + velocity * Geometry.Distance(srcPos, targetPos);
                            npc.GetMovementStateInfo().IsMoving       = true;
                            logic.NotifyNpcMove(npc);
                            data.IsUsingAvoidanceVelocity = false;
                        }
                    }
                    else//改变路点或结束沿路点移动
                    {
                        data.UseNextPathPoint();
                        if (data.HavePathPoint)
                        {
                            targetPos = data.CurPathPoint;
                            npc.GetMovementStateInfo().TargetPosition = targetPos;
                            float angle = Geometry.GetYAngle(new Vector2(srcPos.X, srcPos.Z), new Vector2(targetPos.X, targetPos.Z));
                            npc.GetMovementStateInfo().SetMoveDir(angle);
                            if (faceIsMoveFir)
                            {
                                npc.GetMovementStateInfo().SetFaceDir(angle);
                            }
                            npc.GetMovementStateInfo().IsMoving = true;
                            logic.NotifyNpcMove(npc);
                            data.IsUsingAvoidanceVelocity = false;
                        }
                        else
                        {
                            data.Clear();
                        }
                    }
                }
                if (!havePathPoint || findObstacle)//获得路点过程(寻路)
                {
                    data.Clear();
                    Vector3 targetPos = pathTargetPos;
                    bool    canGo     = true;
                    if (!npc.SpatialSystem.GetCellMapView(npc.AvoidanceRadius).CanPass(targetPos))
                    {
                        if (!AiLogicUtility.GetWalkablePosition(npc.SpatialSystem.GetCellMapView(npc.AvoidanceRadius), targetPos, srcPos, ref targetPos))
                        {
                            canGo = false;
                        }
                    }
                    if (canGo)
                    {
                        List <Vector3> posList = null;
                        bool           canPass = npc.SpatialSystem.CanPass(npc.SpaceObject, targetPos);
                        if (canPass)
                        {
                            posList = new List <Vector3>();
                            posList.Add(srcPos);
                            posList.Add(targetPos);
                        }
                        else
                        {
                            long stTime = TimeUtility.GetElapsedTimeUs();
                            posList = npc.SpatialSystem.FindPath(srcPos, targetPos, npc.AvoidanceRadius);
                            long endTime  = TimeUtility.GetElapsedTimeUs();
                            long calcTime = endTime - stTime;
                            if (calcTime > 10000)
                            {
                                LogSystem.Warn("*** pve FindPath consume {0} us,npc:{1} from:{2} to:{3} radius:{4} pos:{5}", calcTime, npc.GetId(), srcPos.ToString(), targetPos.ToString(), npc.AvoidanceRadius, npc.GetMovementStateInfo().GetPosition3D().ToString());
                            }
                        }
                        if (posList.Count >= 2)
                        {
                            data.SetPathPoints(posList[0], posList, 1);
                            targetPos = data.CurPathPoint;
                            npc.GetMovementStateInfo().TargetPosition = targetPos;
                            float angle = Geometry.GetYAngle(new Vector2(srcPos.X, srcPos.Z), new Vector2(targetPos.X, targetPos.Z));
                            npc.GetMovementStateInfo().SetMoveDir(angle);
                            if (faceIsMoveFir)
                            {
                                npc.GetMovementStateInfo().SetFaceDir(angle);
                            }
                            npc.GetMovementStateInfo().IsMoving = true;
                            logic.NotifyNpcMove(npc);
                            data.IsUsingAvoidanceVelocity = false;
                        }
                        else
                        {
                            npc.GetMovementStateInfo().IsMoving = false;
                            logic.NotifyNpcMove(npc);
                            data.IsUsingAvoidanceVelocity = false;
                        }
                    }
                    else
                    {
                        npc.GetMovementStateInfo().IsMoving = false;
                        logic.NotifyNpcMove(npc);
                        data.IsUsingAvoidanceVelocity = false;
                    }
                }
            }
        }
Beispiel #20
0
    public override void Iterate()
    {
        if (m_body1.inv_m == 0.0f && m_body2.inv_m == 0.0f)
        {
            return;
        }

        // Compute the relative velocity between the bodies
        Vector3 relativeVelocity = (m_body2.v + Vector3.Cross(m_body2.omega, MyMath.Rotate(m_body2.q, m_r2))) -
                                   (m_body1.v + Vector3.Cross(m_body1.omega, MyMath.Rotate(m_body1.q, m_r1)));

        // Project the relative velocity onto the constraint direction
        float velocityError = Vector3.Dot(relativeVelocity, m_velocityConstraintDirection);

        // Compute the velocity delta needed to satisfy the constraint
        float deltaVelocity = -velocityError - m_positionError;

        // Compute the momentum to be exchanged to correct velocities
        float momentumPacket = deltaVelocity * m_effectiveMass;

        // Clamp the momentum packet to reflect the fact that the contact can only push the objects apart
        momentumPacket = Math.Min(momentumPacket, -m_cachedMomentum);

        Vector3 momentumPacketWithDir = momentumPacket * m_velocityConstraintDirection;

        // Exchange the correctional momentum between the bodies
        m_body1.v     -= momentumPacketWithDir * m_body1.inv_m;
        m_body2.v     += momentumPacketWithDir * m_body2.inv_m;
        m_body1.omega -= momentumPacket * m_invMoment1;
        m_body2.omega += momentumPacket * m_invMoment2;
        Debug_c.Valid(m_body1.omega);
        Debug_c.Valid(m_body2.omega);

        // Test code
        //Vector3 newRelativeVelocity = (m_body2.v + Vector3.Cross(m_body2.omega, MyMath.Rotate(m_body2.q, m_r2))) -
        //							  (m_body1.v + Vector3.Cross(m_body1.omega, MyMath.Rotate(m_body1.q, m_r1)));
        //float newVelocityError = Vector3.Dot(newRelativeVelocity, m_velocityConstraintDirection);

        // Accumulate the momentum for next frame
        m_cachedMomentum += momentumPacket;

        ///
        // FRICTION

        //if (gFriction)
        {
            relativeVelocity = (m_body2.v + Vector3.Cross(m_body2.omega, MyMath.Rotate(m_body2.q, m_r2))) -
                               (m_body1.v + Vector3.Cross(m_body1.omega, MyMath.Rotate(m_body1.q, m_r1)));

            Vector3 tangentVelocityDirection = relativeVelocity - Vector3.Dot(relativeVelocity, m_velocityConstraintDirection) * m_velocityConstraintDirection;
            if (tangentVelocityDirection.LengthSquared() < 0.0001f)
            {
                return;
            }
            Debug_c.Valid(tangentVelocityDirection);
            tangentVelocityDirection.Normalize();

            float tangentVelocityError = Vector3.Dot(relativeVelocity, tangentVelocityDirection);

            Vector3 r1 = MyMath.Rotate(m_body1.q, m_r1);
            Vector3 r2 = MyMath.Rotate(m_body2.q, m_r2);

            Vector3 invMoment1 = Vector3.Transform(Vector3.Cross(r1, tangentVelocityDirection), m_body1.inv_I);
            Vector3 invMoment2 = Vector3.Transform(Vector3.Cross(r2, tangentVelocityDirection), m_body2.inv_I);
            Debug_c.Valid(invMoment1);
            Debug_c.Valid(invMoment2);

            // Compute effective mass of the constraint system -- this is a measure of how easy it
            // is to accelerate the contact points apart along the constraint direction -- it's analogous
            // to effective resistance in an electric circuit [i.e., 1 / (1/R1 + 1/R2)]
            float effectiveMass =
                1.0f /
                (
                    m_body1.inv_m +
                    m_body2.inv_m +
                    Vector3.Dot(tangentVelocityDirection,
                                (
                                    Vector3.Cross(invMoment1, r1) +
                                    Vector3.Cross(invMoment2, r2)
                                ))
                );

            float tangentDeltaVelocity = -tangentVelocityError;

            float tangentMomentumPacket = tangentDeltaVelocity * effectiveMass;

            tangentMomentumPacket = Math.Max(tangentMomentumPacket, m_cachedMomentum * 0.5f);

            Vector3 tangentMomentumPacketWithDir = tangentMomentumPacket * tangentVelocityDirection;

            // Exchange the correctional momentum between the bodies
            m_body1.v     -= tangentMomentumPacketWithDir * m_body1.inv_m;
            m_body2.v     += tangentMomentumPacketWithDir * m_body2.inv_m;
            m_body1.omega -= tangentMomentumPacket * invMoment1;
            m_body2.omega += tangentMomentumPacket * invMoment2;

            Debug_c.Valid(m_body1.v);
            Debug_c.Valid(m_body2.v);
            Debug_c.Valid(m_body1.omega);
            Debug_c.Valid(m_body2.omega);

            m_cachedTangentMomentum = tangentMomentumPacketWithDir;
        }
    }
Beispiel #21
0
 public bool PointIsContained(ref Vector3 sampleSpacing, ref Vector3 point)
 {
     return(point.LengthSquared() <= Sphere.Radius * Sphere.Radius);
 }
Beispiel #22
0
    public void GenerateImpulse(float e /*Bouncyness (Coefficient of restitution)*/)
    {
        if (m_body1.inv_m == 0.0f && m_body2.inv_m == 0.0f)
        {
            return;
        }

        Vector3 v1 = m_body1.v + Vector3.Cross(m_body1.omega, MyMath.Rotate(m_body1.q, m_r1));
        Vector3 v2 = m_body2.v + Vector3.Cross(m_body2.omega, MyMath.Rotate(m_body2.q, m_r2));


        // Compute the relative velocity between the bodies
        Vector3 relativeVelocity = v2 - v1;

        // If the objects are moving away from each other we dont need to apply an impulse
        float relativeMovement = Vector3.Dot(relativeVelocity, m_velocityConstraintDirection);

        if (relativeMovement < -0.01f)
        {
            return;
        }


        Vector3 r1 = MyMath.Rotate(m_body1.q, m_r1);
        Vector3 r2 = MyMath.Rotate(m_body2.q, m_r2);

        //Vector3 r1 = m_body1.x + MyMath.Rotate(m_body1.q, m_r1);
        //Vector3 r2 = m_body2.x + MyMath.Rotate(m_body2.q, m_r2);

        //Vector3 r1 = m_r1;
        //Vector3 r2 = m_r2;

        Matrix orientationMatrix1         = Matrix.CreateFromQuaternion(m_body1.q);
        Matrix inverseOrientationMatrix1  = Matrix.Transpose(orientationMatrix1);
        Matrix inverseWorldInertiaMatrix1 = orientationMatrix1 * m_body1.inv_I * inverseOrientationMatrix1;
        //Matrix inverseWorldInertiaMatrix1 = inverseOrientationMatrix1 * m_body1.inv_I * orientationMatrix1;
        //Matrix inverseWorldInertiaMatrix1 = m_body1.inv_I;

        Matrix orientationMatrix2         = Matrix.CreateFromQuaternion(m_body2.q);
        Matrix inverseOrientationMatrix2  = Matrix.Transpose(orientationMatrix2);
        Matrix inverseWorldInertiaMatrix2 = orientationMatrix2 * m_body2.inv_I * inverseOrientationMatrix2;
        //Matrix inverseWorldInertiaMatrix2 = inverseOrientationMatrix2 * m_body2.inv_I * orientationMatrix2;
        //Matrix inverseWorldInertiaMatrix2 = m_body2.inv_I;


        Vector3 a1 = Vector3.Transform(Vector3.Cross(r1, m_velocityConstraintDirection), inverseWorldInertiaMatrix1);
        Vector3 a2 = Vector3.Transform(Vector3.Cross(r2, m_velocityConstraintDirection), inverseWorldInertiaMatrix2);

        //Vector3 a1 = Vector3.Transform(Vector3.Cross(r1, m_velocityConstraintDirection), m_body1.inv_I);
        //Vector3 a2 = Vector3.Transform(Vector3.Cross(r2, m_velocityConstraintDirection), m_body2.inv_I);

        float kn = m_body1.inv_m + m_body2.inv_m +
                   Vector3.Dot(m_velocityConstraintDirection, Vector3.Cross(a1, r1)) +
                   Vector3.Dot(m_velocityConstraintDirection, Vector3.Cross(a2, r2));

        float pn = (1 + e) / kn;

        Vector3 J = -m_velocityConstraintDirection * relativeMovement * (1 + e) / kn;


        //m_velocityConstraintDirection = Vector3.Normalize( m_velocityConstraintDirection );
        //J = Vector3.Dot( J, m_velocityConstraintDirection ) * m_velocityConstraintDirection;


        //J = Vector3.Normalize( J ) * len;
        //float len2 = J.Length();


        m_body1.v -= J * m_body1.inv_m;
        m_body2.v += J * m_body2.inv_m;
        Vector3 oldOmega1 = m_body1.omega;
        Vector3 oldOmega2 = m_body2.omega;

        m_body1.omega = oldOmega1 - Vector3.Transform(Vector3.Cross(r1, J), inverseWorldInertiaMatrix1);
        m_body2.omega = oldOmega2 + Vector3.Transform(Vector3.Cross(r2, J), inverseWorldInertiaMatrix2);
        //m_body1.omega	= oldOmega1 - Vector3.Transform(Vector3.Cross(r1, J), m_body1.inv_I);
        //m_body2.omega	= oldOmega2 + Vector3.Transform(Vector3.Cross(r2, J), m_body2.inv_I);

        Debug_c.Valid(m_body1.v);
        Debug_c.Valid(m_body2.v);
        Debug_c.Valid(m_body1.omega);
        Debug_c.Valid(m_body2.omega);

        // Tangent Friction
        //if (false)
        {
            // Work out our tangent vector, with is perpendicular
            // to our collision normal
            Vector3 tangent = relativeVelocity - Vector3.Dot(relativeVelocity, m_velocityConstraintDirection) * m_velocityConstraintDirection;

            if (tangent.LengthSquared() < 0.00001f)
            {
                return;
            }
            tangent.Normalize();

            Vector3 at1 = Vector3.Transform(Vector3.Cross(r1, tangent), inverseWorldInertiaMatrix1);
            Vector3 at2 = Vector3.Transform(Vector3.Cross(r2, tangent), inverseWorldInertiaMatrix2);

            float ktn = m_body1.inv_m + m_body2.inv_m +
                        Vector3.Dot(tangent, Vector3.Cross(at1, r1)) +
                        Vector3.Dot(tangent, Vector3.Cross(at2, r2));

            //float mu = 0.5f;

            float pt = (1 + e) / ktn;

            pt = MathHelper.Clamp(pt, -0.3f * pn, 0.3f * pn);
            Vector3 Jt = -tangent * pt;



            m_body1.v -= Jt * m_body1.inv_m;
            m_body2.v += Jt * m_body2.inv_m;
            Vector3 oldOmegat1 = m_body1.omega;
            Vector3 oldOmegat2 = m_body2.omega;
            m_body1.omega = oldOmegat1 - Vector3.Transform(Vector3.Cross(r1, Jt), inverseWorldInertiaMatrix1);
            m_body2.omega = oldOmegat2 + Vector3.Transform(Vector3.Cross(r2, Jt), inverseWorldInertiaMatrix2);

            Debug_c.Valid(m_body1.v);
            Debug_c.Valid(m_body2.v);
            Debug_c.Valid(m_body1.omega);
            Debug_c.Valid(m_body2.omega);
        }
    }
Beispiel #23
0
        /// <summary>
        /// Updates the collection of supporting contacts.
        /// </summary>
        public void UpdateSupports(ref Vector3 movementDirection)
        {
            bool hadTraction = HasTraction;

            //Reset traction/support.
            HasTraction = false;
            HasSupport  = false;

            Vector3 downDirection = characterBody.orientationMatrix.Down;
            Vector3 bodyPosition  = characterBody.position;

            //Compute the character's radius, minus a little margin. We want the rays to originate safely within the character's body.
            //Assume vertical rotational invariance. Spheres, cylinders, and capsules don't have varying horizontal radii.
            Vector3 extremePoint;
            var     convexShape = characterBody.CollisionInformation.Shape as ConvexShape;

            Debug.Assert(convexShape != null, "Character bodies must be convex.");

            //Find the lowest point on the collision shape.
            convexShape.GetLocalExtremePointWithoutMargin(ref Toolbox.DownVector, out extremePoint);
            BottomDistance = -extremePoint.Y + convexShape.collisionMargin;

            convexShape.GetLocalExtremePointWithoutMargin(ref Toolbox.RightVector, out extremePoint);
            float rayCastInnerRadius = Math.Max((extremePoint.X + convexShape.collisionMargin) * 0.8f, extremePoint.X);

            //Vertically, the rays will start at the same height as the character's center.
            //While they could be started lower on a cylinder, that wouldn't always work for a sphere or capsule: the origin might end up outside of the shape!

            tractionContacts.Clear();
            supportContacts.Clear();
            sideContacts.Clear();
            headContacts.Clear();

            foreach (var pair in characterBody.CollisionInformation.Pairs)
            {
                //Don't stand on things that aren't really colliding fully.
                if (pair.CollisionRule != CollisionRule.Normal)
                {
                    continue;
                }
                ContactCategorizer.CategorizeContacts(pair, characterBody.CollisionInformation, ref downDirection, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts);
            }

            HasSupport  = supportContacts.Count > 0;
            HasTraction = tractionContacts.Count > 0;

            //Only perform ray casts if the character has fully left the surface, and only if the previous frame had traction.
            //(If ray casts are allowed when support contacts still exist, the door is opened for climbing surfaces which should not be climbable.
            //Consider a steep slope. If the character runs at it, the character will likely be wedged off of the ground, making it lose traction while still having a support contact with the slope.
            //If ray tests are allowed when support contacts exist, the character will maintain traction despite climbing the wall.
            //The VerticalMotionConstraint can stop the character from climbing in many cases, but it's nice not to have to rely on it.
            //Disallowing ray tests when supports exist does have a cost, though. For example, consider rounded steps.
            //If the character walks off a step such that it is still in contact with the step but is far enough down that the slope is too steep for traction,
            //the ray test won't recover traction. This situation just isn't very common.)
            if (!HasSupport && hadTraction)
            {
                float supportRayLength = maximumAssistedDownStepHeight + BottomDistance;
                SupportRayData = null;
                //If the contacts aren't available to support the character, raycast down to find the ground.
                if (!HasTraction)
                {
                    //TODO: could also require that the character has a nonzero movement direction in order to use a ray cast.  Questionable- would complicate the behavior on edges.
                    Ray ray = new Ray(bodyPosition, downDirection);

                    bool           hasTraction;
                    SupportRayData data;
                    if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data))
                    {
                        SupportRayData = data;
                        HasTraction    = data.HasTraction;
                        HasSupport     = true;
                    }
                }

                //If contacts and the center ray cast failed, try a ray offset in the movement direction.
                bool tryingToMove = movementDirection.LengthSquared() > 0;
                if (!HasTraction && tryingToMove)
                {
                    Ray ray = new Ray(
                        characterBody.Position +
                        movementDirection * rayCastInnerRadius, downDirection);

                    //Have to test to make sure the ray doesn't get obstructed.  This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support!
                    Ray obstructionRay;
                    obstructionRay.Position  = characterBody.Position;
                    obstructionRay.Direction = ray.Position - obstructionRay.Position;
                    if (!QueryManager.RayCastHitAnything(obstructionRay, 1))
                    {
                        //The origin isn't obstructed, so now ray cast down.
                        bool           hasTraction;
                        SupportRayData data;
                        if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data))
                        {
                            if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T)
                            {
                                //Only replace the previous support ray if we now have traction or we didn't have a support ray at all before,
                                //or this hit is a better (sooner) hit.
                                if (hasTraction)
                                {
                                    SupportRayData = data;
                                    HasTraction    = true;
                                }
                                else if (SupportRayData == null)
                                {
                                    SupportRayData = data;
                                }
                                HasSupport = true;
                            }
                        }
                    }
                }

                //If contacts, center ray, AND forward ray failed to find traction, try a side ray created from down x forward.
                if (!HasTraction && tryingToMove)
                {
                    //Compute the horizontal offset direction.  Down direction and the movement direction are normalized and perpendicular, so the result is too.
                    Vector3 horizontalOffset;
                    Vector3.Cross(ref movementDirection, ref downDirection, out horizontalOffset);
                    Vector3.Multiply(ref horizontalOffset, rayCastInnerRadius, out horizontalOffset);
                    Ray ray = new Ray(bodyPosition + horizontalOffset, downDirection);

                    //Have to test to make sure the ray doesn't get obstructed.  This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support!
                    Ray obstructionRay;
                    obstructionRay.Position  = bodyPosition;
                    obstructionRay.Direction = ray.Position - obstructionRay.Position;
                    if (!QueryManager.RayCastHitAnything(obstructionRay, 1))
                    {
                        //The origin isn't obstructed, so now ray cast down.
                        bool           hasTraction;
                        SupportRayData data;
                        if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data))
                        {
                            if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T)
                            {
                                //Only replace the previous support ray if we now have traction or we didn't have a support ray at all before,
                                //or this hit is a better (sooner) hit.
                                if (hasTraction)
                                {
                                    SupportRayData = data;
                                    HasTraction    = true;
                                }
                                else if (SupportRayData == null)
                                {
                                    SupportRayData = data;
                                }
                                HasSupport = true;
                            }
                        }
                    }
                }

                //If contacts, center ray, forward ray, AND the first side ray failed to find traction, try a side ray created from forward x down.
                if (!HasTraction && tryingToMove)
                {
                    //Compute the horizontal offset direction.  Down direction and the movement direction are normalized and perpendicular, so the result is too.
                    Vector3 horizontalOffset;
                    Vector3.Cross(ref downDirection, ref movementDirection, out horizontalOffset);
                    Vector3.Multiply(ref horizontalOffset, rayCastInnerRadius, out horizontalOffset);
                    Ray ray = new Ray(bodyPosition + horizontalOffset, downDirection);

                    //Have to test to make sure the ray doesn't get obstructed.  This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support!
                    Ray obstructionRay;
                    obstructionRay.Position  = bodyPosition;
                    obstructionRay.Direction = ray.Position - obstructionRay.Position;
                    if (!QueryManager.RayCastHitAnything(obstructionRay, 1))
                    {
                        //The origin isn't obstructed, so now ray cast down.
                        bool           hasTraction;
                        SupportRayData data;
                        if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data))
                        {
                            if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T)
                            {
                                //Only replace the previous support ray if we now have traction or we didn't have a support ray at all before,
                                //or this hit is a better (sooner) hit.
                                if (hasTraction)
                                {
                                    SupportRayData = data;
                                    HasTraction    = true;
                                }
                                else if (SupportRayData == null)
                                {
                                    SupportRayData = data;
                                }
                                HasSupport = true;
                            }
                        }
                    }
                }
            }

            UpdateSupportData(ref downDirection);
            UpdateVerticalSupportData(ref downDirection, ref movementDirection);
        }
Beispiel #24
0
    public Frustum(
        TScalar aspectRatio,
        Vector3 <TScalar> axis,
        TScalar fieldOfViewAngle,
        TScalar nearPlaneDistance,
        Vector3 <TScalar> position,
        Quaternion <TScalar> rotation)
    {
        AspectRatio       = aspectRatio;
        Axis              = axis;
        FieldOfViewAngle  = fieldOfViewAngle;
        NearPlaneDistance = nearPlaneDistance;
        Position          = position;
        Rotation          = rotation;

        var farPlaneDistSq = Vector3 <TScalar> .LengthSquared(Axis);

        FarPlaneDistance = TScalar.Sqrt(farPlaneDistSq);

        var two    = NumberValues.Two <TScalar>();
        var tan    = TScalar.Tan(fieldOfViewAngle);
        var tanSq4 = tan.Square() * NumberValues.Four <TScalar>();
        var tanAR  = tan * aspectRatio;

        Volume = tanSq4
                 * aspectRatio
                 * ((farPlaneDistSq * FarPlaneDistance) - nearPlaneDistance.Cube())
                 / NumberValues.Three <TScalar>();

        SmallestDimension = aspectRatio <= TScalar.One
            ? two * tanAR * nearPlaneDistance
            : two * tan * nearPlaneDistance;

        ContainingRadius = TScalar.Sqrt(
            (FarPlaneDistance - nearPlaneDistance).Square()
            + (tanSq4 * farPlaneDistSq * (TScalar.One + aspectRatio.Square())))
                           / two;

        axis = position - (axis / two);
        var basis1 = Vector3 <TScalar> .Transform(Vector3 <TScalar> .Normalize(axis), rotation);

        Vector3 <TScalar> basis2, basis3;

        if (basis1.Z.IsNearlyEqualTo(TScalar.NegativeOne))
        {
            basis2 = new Vector3 <TScalar>(TScalar.Zero, TScalar.NegativeOne, TScalar.Zero);
            basis3 = new Vector3 <TScalar>(TScalar.NegativeOne, TScalar.Zero, TScalar.Zero);
        }
        else
        {
            var a = TScalar.One / (TScalar.One + basis1.Z);
            var b = -basis1.X * basis1.Y * a;
            basis2 = new Vector3 <TScalar>(TScalar.One - (basis1.X.Square() * a), b, -basis1.X);
            basis3 = new Vector3 <TScalar>(b, TScalar.One - (basis1.Y.Square() * a), -basis1.Y);
        }
        var farY  = basis2 * farPlaneDistSq * tanAR;
        var farZ  = basis3 * tan * FarPlaneDistance;
        var nearX = basis1 * nearPlaneDistance;
        var nearY = basis2 * nearPlaneDistance.Square() * tanAR;
        var nearZ = basis3 * tan * nearPlaneDistance;

        Corners = new Vector3 <TScalar>[8]
        {
            Position + axis - farY + farZ,
            Position + axis + farY + farZ,
            Position + axis - farY - farZ,
            Position + axis + farY - farZ,
            Position + nearX + nearY - nearZ,
            Position + nearX - nearY - nearZ,
            Position + nearX - nearY + nearZ,
            Position + nearX + nearY + nearZ
        };

        HighestPoint = Corners[0];
        LowestPoint  = Corners[0];
        for (var i = 1; i < 8; i++)
        {
            if (Corners[i].Y > HighestPoint.Y)
            {
                HighestPoint = Corners[i];
            }
            if (Corners[i].Y < LowestPoint.Y)
            {
                LowestPoint = Corners[i];
            }
        }

        Planes = new Plane <TScalar>[6]
        {
            Plane <TScalar> .CreateFromVertices(Corners[0], Corners[1], Corners[2]),
            Plane <TScalar> .CreateFromVertices(Corners[0], Corners[1], Corners[4]),
            Plane <TScalar> .CreateFromVertices(Corners[0], Corners[1], Corners[5]),
            Plane <TScalar> .CreateFromVertices(Corners[0], Corners[1], Corners[6]),
            Plane <TScalar> .CreateFromVertices(Corners[0], Corners[1], Corners[7]),
            Plane <TScalar> .CreateFromVertices(Corners[4], Corners[5], Corners[6]),
        };
    }
Beispiel #25
0
        public override void Update(float dt)
        {
            base.Update(dt);
            ////Rotate the camera of the character based on the support velocity, if a support with velocity exists.
            ////This can be very disorienting in some cases; that's why it is off by default!
            //if (Character.SupportFinder.HasSupport)
            //{
            //    SupportData? data;
            //    if (Character.SupportFinder.HasTraction)
            //        data = Character.SupportFinder.TractionData;
            //    else
            //        data = Character.SupportFinder.SupportData;
            //    var support = data.Value.SupportObject as EntityCollidable;
            //    if (support != null && !support.Entity.IsDynamic) //Having the view turned by dynamic entities is extremely confusing for the most part.
            //    {
            //        float dot = Vector3.Dot(support.Entity.AngularVelocity, Character.Body.OrientationMatrix.Up);
            //        Camera.Yaw(dot * dt);
            //    }
            //}

            if (UseCameraSmoothing)
            {
                //First, find where the camera is expected to be based on the last position and the current velocity.
                //Note: if the character were a free-floating 6DOF character, this would need to include an angular velocity contribution.
                //And of course, the camera orientation would be based on the character's orientation.

                //We use the space's time step since, in the demos, the simulation moves forward one time step per frame.
                //The passed-in dt, in contrast, does not necessarily correspond to a simulated state and tends to make the camera jittery.
                var spaceDt = Character.Space != null ? Character.Space.TimeStepSettings.TimeStepDuration : dt;
                Camera.Position = Camera.Position + Character.Body.LinearVelocity * spaceDt;
                //Now compute where it should be according the physical body of the character.
                Vector3 up           = Character.Body.OrientationMatrix.Up;
                Vector3 bodyPosition = Character.Body.BufferedStates.InterpolatedStates.Position;
                Vector3 goalPosition = bodyPosition + up * (Character.StanceManager.CurrentStance == Stance.Standing ? StandingCameraOffset : CrouchingCameraOffset);

                //Usually, the camera position and the goal will be very close, if not matching completely.
                //However, if the character steps or has its position otherwise modified, then they will not match.
                //In this case, we need to correct the camera position.

                //To do this, first note that we can't correct infinite errors.  We need to define a bounding region that is relative to the character
                //in which the camera can interpolate around.  The most common discontinuous motions are those of upstepping and downstepping.
                //In downstepping, the character can teleport up to the character's MaximumStepHeight downwards.
                //In upstepping, the character can teleport up to the character's MaximumStepHeight upwards, and the body's CollisionMargin horizontally.
                //Picking those as bounds creates a constraining cylinder.

                vxConsole.WriteToInGameDebug("MaximumStepHeight: " + Character.StepManager.MaximumStepHeight);
                Vector3 error           = goalPosition - Camera.Position;
                float   verticalError   = Vector3.Dot(error, up);
                Vector3 horizontalError = error - verticalError * up;
                //Clamp the vertical component of the camera position within the bounding cylinder.
                if (verticalError > Character.StepManager.MaximumStepHeight)
                {
                    Camera.Position -= up * (Character.StepManager.MaximumStepHeight - verticalError);
                    verticalError    = Character.StepManager.MaximumStepHeight;
                }
                else if (verticalError < -Character.StepManager.MaximumStepHeight)
                {
                    Camera.Position -= up * (-Character.StepManager.MaximumStepHeight - verticalError);
                    verticalError    = -Character.StepManager.MaximumStepHeight;
                }
                //Clamp the horizontal distance too.
                float horizontalErrorLength = horizontalError.LengthSquared();
                float margin = Character.Body.CollisionInformation.Shape.CollisionMargin;
                if (horizontalErrorLength > margin * margin)
                {
                    Vector3 previousHorizontalError = horizontalError;
                    Vector3.Multiply(ref horizontalError, margin / (float)Math.Sqrt(horizontalErrorLength), out horizontalError);
                    Camera.Position -= horizontalError - previousHorizontalError;
                }
                //Now that the error/camera position is known to lie within the constraining cylinder, we can perform a smooth correction.

                //This removes a portion of the error each frame.
                //Note that this is not framerate independent.  If fixed time step is not enabled,
                //a different smoothing method should be applied to the final error values.
                //float errorCorrectionFactor = .3f;

                //This version is framerate independent, although it is more expensive.
                float errorCorrectionFactor = (float)(1 - Math.Pow(.000000001, dt));
                Camera.Position += up * (verticalError * errorCorrectionFactor);
                Camera.Position += horizontalError * errorCorrectionFactor;
            }
            else
            {
                Camera.Position = Character.Body.Position + (Character.StanceManager.CurrentStance == Stance.Standing ? StandingCameraOffset : CrouchingCameraOffset) * Character.Body.OrientationMatrix.Up;
            }
        }
Beispiel #26
0
        ///<summary>
        /// Adds a new point to the simplex.
        ///</summary>
        ///<param name="shapeA">First shape in the pair.</param>
        ///<param name="shapeB">Second shape in the pair.</param>
        ///<param name="iterationCount">Current iteration count.</param>
        ///<param name="closestPoint">Current point on simplex closest to origin.</param>
        ///<returns>Whether or not GJK should exit due to a lack of progression.</returns>
        public bool GetNewSimplexPoint(ConvexShape shapeA, ConvexShape shapeB, int iterationCount,
                                       ref Vector3 closestPoint)
        {
            Vector3 negativeDirection;

            Vector3.Negate(ref closestPoint, out negativeDirection);
            Vector3 sa, sb;

            shapeA.GetLocalExtremePointWithoutMargin(ref negativeDirection, out sa);
            shapeB.GetExtremePointWithoutMargin(closestPoint, ref LocalTransformB, out sb);
            Vector3 S;

            Vector3.Subtract(ref sa, ref sb, out S);
            //If S is not further towards the origin along negativeDirection than closestPoint, then we're done.
            float dotS;

            Vector3.Dot(ref S, ref negativeDirection, out dotS); //-P * S
            float distanceToClosest = closestPoint.LengthSquared();

            float progression = dotS + distanceToClosest;

            //It's likely that the system is oscillating between two or more states, usually because of a degenerate simplex.
            //Rather than detect specific problem cases, this approach just lets it run and catches whatever falls through.
            //During oscillation, one of the states is usually just BARELY outside of the numerical tolerance.
            //After a bunch of iterations, the system lets it pick the 'better' one.
            if (iterationCount > GJKToolbox.HighGJKIterations && distanceToClosest - previousDistanceToClosest <
                DistanceConvergenceEpsilon * errorTolerance)
            {
                return(true);
            }

            if (distanceToClosest < previousDistanceToClosest)
            {
                previousDistanceToClosest = distanceToClosest;
            }

            //If "A" is the new point always, then the switch statement can be removed
            //in favor of just pushing three points up.
            switch (State)
            {
            case SimplexState.Point:
                if (progression <= (errorTolerance = MathHelper.Max(A.LengthSquared(), S.LengthSquared())) *
                    ProgressionEpsilon)
                {
                    return(true);
                }

                State      = SimplexState.Segment;
                B          = S;
                SimplexA.B = sa;
                SimplexB.B = sb;
                return(false);

            case SimplexState.Segment:
                if (progression <=
                    (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()),
                                                     S.LengthSquared())) * ProgressionEpsilon)
                {
                    return(true);
                }

                State      = SimplexState.Triangle;
                C          = S;
                SimplexA.C = sa;
                SimplexB.C = sb;
                return(false);

            case SimplexState.Triangle:
                if (progression <=
                    (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()),
                                                     MathHelper.Max(C.LengthSquared(), S.LengthSquared()))) * ProgressionEpsilon)
                {
                    return(true);
                }

                State      = SimplexState.Tetrahedron;
                D          = S;
                SimplexA.D = sa;
                SimplexB.D = sb;
                return(false);
            }

            return(false);
        }
        //  Update position, check collisions, etc. and draw if particle still lives.
        //  Return false if particle dies/timeouts in this tick.
        public bool Draw(VRageRender.MyBillboard billboard)
        {
            var actualPosition = m_actualPosition + m_actualPivot;

            MyTransparentGeometry.StartParticleProfilingBlock("Distance calculation");
            //  This time is scaled according to planned lifespan of the particle

            // Distance for sorting
            billboard.DistanceSquared = (float)Vector3D.DistanceSquared(MyTransparentGeometry.Camera.Translation, actualPosition);

            MyTransparentGeometry.EndParticleProfilingBlock();

            // If distance to camera is really small don't draw it.
            if (billboard.DistanceSquared <= 0.1f)
            {
                return(false);
            }

            MyTransparentGeometry.StartParticleProfilingBlock("Quad calculation");

            MyTransparentGeometry.StartParticleProfilingBlock("actualRadius");
            float actualRadius = 1;

            Radius.GetInterpolatedValue <float>(m_normalizedTime, out actualRadius);
            MyTransparentGeometry.EndParticleProfilingBlock();

            billboard.ContainedBillboards.Clear();

            billboard.Near   = m_generation.GetEffect().Near;
            billboard.Lowres = m_generation.GetEffect().LowRes || VRageRender.MyRenderConstants.RenderQualityProfile.LowResParticles;
            billboard.CustomViewProjection = -1;
            billboard.ParentID             = -1;

            float alpha = 1;

            if (Type == MyParticleTypeEnum.Point)
            {
                MyTransparentGeometry.StartParticleProfilingBlock("GetBillboardQuadRotated");
                GetBillboardQuadRotated(billboard, ref actualPosition, actualRadius, m_actualAngle);
                MyTransparentGeometry.EndParticleProfilingBlock();
            }
            else if (Type == MyParticleTypeEnum.Line)
            {
                if (MyUtils.IsZero(Velocity.LengthSquared()))
                {
                    Velocity = MyUtils.GetRandomVector3Normalized();
                }

                MyQuadD quad = new MyQuadD();

                MyPolyLineD polyLine = new MyPolyLineD();
                polyLine.LineDirectionNormalized = MyUtils.Normalize(Velocity);

                if (m_actualAngle > 0)
                {
                    polyLine.LineDirectionNormalized = Vector3.TransformNormal(polyLine.LineDirectionNormalized, Matrix.CreateRotationY(MathHelper.ToRadians(m_actualAngle)));
                }

                polyLine.Point0   = actualPosition;
                polyLine.Point1.X = actualPosition.X + polyLine.LineDirectionNormalized.X * actualRadius;
                polyLine.Point1.Y = actualPosition.Y + polyLine.LineDirectionNormalized.Y * actualRadius;
                polyLine.Point1.Z = actualPosition.Z + polyLine.LineDirectionNormalized.Z * actualRadius;

                if (m_actualAngle > 0)
                { //centerize
                    polyLine.Point0.X = polyLine.Point0.X - polyLine.LineDirectionNormalized.X * actualRadius * 0.5f;
                    polyLine.Point0.Y = polyLine.Point0.Y - polyLine.LineDirectionNormalized.Y * actualRadius * 0.5f;
                    polyLine.Point0.Z = polyLine.Point0.Z - polyLine.LineDirectionNormalized.Z * actualRadius * 0.5f;
                    polyLine.Point1.X = polyLine.Point1.X - polyLine.LineDirectionNormalized.X * actualRadius * 0.5f;
                    polyLine.Point1.Y = polyLine.Point1.Y - polyLine.LineDirectionNormalized.Y * actualRadius * 0.5f;
                    polyLine.Point1.Z = polyLine.Point1.Z - polyLine.LineDirectionNormalized.Z * actualRadius * 0.5f;
                }

                polyLine.Thickness = Thickness;
                var camPos = MyTransparentGeometry.Camera.Translation;
                MyUtils.GetPolyLineQuad(out quad, ref polyLine, camPos);

                if (this.m_generation.AlphaAnisotropic)
                {
                    float angle     = 1 - Math.Abs(Vector3.Dot(MyUtils.Normalize(MyTransparentGeometry.Camera.Forward), polyLine.LineDirectionNormalized));
                    float alphaCone = (float)Math.Pow(angle, 0.5f);
                    alpha = alphaCone;
                }

                billboard.Position0 = quad.Point0;
                billboard.Position1 = quad.Point1;
                billboard.Position2 = quad.Point2;
                billboard.Position3 = quad.Point3;
            }
            else if (Type == MyParticleTypeEnum.Trail)
            {
                if (Quad.Point0 == Quad.Point2) //not moving particle
                {
                    return(false);
                }
                if (Quad.Point1 == Quad.Point3) //not moving particle was previous one
                {
                    return(false);
                }
                if (Quad.Point0 == Quad.Point3) //not moving particle was previous one
                {
                    return(false);
                }

                billboard.Position0 = Quad.Point0;
                billboard.Position1 = Quad.Point1;
                billboard.Position2 = Quad.Point2;
                billboard.Position3 = Quad.Point3;

                //if (this.m_generation.AlphaAnisotropic)

                /*   { //Trails are anisotropic by default (nobody wants them to see ugly)
                 *     Vector3 lineDir = Vector3.Normalize(Quad.Point1 - Quad.Point0);
                 *     float angle = 1 - Math.Abs(Vector3.Dot(MyMwcUtils.Normalize(MyCamera.ForwardVector), lineDir));
                 *     float alphaCone = (float)Math.Pow(angle, 0.3f);
                 *     alpha = alphaCone;
                 * }*/
            }
            else
            {
                throw new NotSupportedException(Type + " is not supported particle type");
            }

            MyTransparentGeometry.EndParticleProfilingBlock();

            MyTransparentGeometry.StartParticleProfilingBlock("Material calculation");

            Vector4 color;

            Color.GetInterpolatedValue <Vector4>(m_normalizedTime, out color);

            var   material1         = MyTransparentMaterials.GetMaterial("ErrorMaterial");
            var   material2         = MyTransparentMaterials.GetMaterial("ErrorMaterial");
            float textureBlendRatio = 0;

            if ((Flags & ParticleFlags.BlendTextures) != 0)
            {
                float prevTime, nextTime, difference;
                Material.GetPreviousValue(m_normalizedTime, out material1, out prevTime);
                Material.GetNextValue(m_normalizedTime, out material2, out nextTime, out difference);

                if (prevTime != nextTime)
                {
                    textureBlendRatio = (m_normalizedTime - prevTime) * difference;
                }
            }
            else
            {
                Material.GetInterpolatedValue(m_normalizedTime, out material1);
            }

            MyTransparentGeometry.EndParticleProfilingBlock();

            //This gets 0.44ms for 2000 particles
            MyTransparentGeometry.StartParticleProfilingBlock("billboard.Start");


            billboard.Material          = material1.Name;
            billboard.BlendMaterial     = material2.Name;
            billboard.BlendTextureRatio = textureBlendRatio;
            billboard.EnableColorize    = false;

            billboard.Color = color * alpha * m_generation.GetEffect().UserColorMultiplier;

            MyTransparentGeometry.EndParticleProfilingBlock();

            return(true);
        }
        public void MoveObject(ulong id, BoundingBoxD oldAabb, BoundingBoxD aabb, Vector3 velocity)
        {
            System.Diagnostics.Debug.Assert(id != CLUSTERED_OBJECT_ID_UNITIALIZED, "Unitialized object in cluster!");

            MyObjectData objectData;

            if (m_objectsData.TryGetValue(id, out objectData))
            {
                System.Diagnostics.Debug.Assert(!objectData.ActivationHandler.IsStaticForCluster, "Cannot move static object!");

                var oldAABB = objectData.AABB;
                m_objectsData[id].AABB = aabb;

                BoundingBoxD originalAABB = aabb;
                Vector3      velocityDir  = velocity;

                if (velocity.LengthSquared() > 0.001f)
                {
                    velocityDir = Vector3.Normalize(velocity);
                }

                BoundingBoxD extendedAABB = aabb.Include(aabb.Center + velocityDir * 2000);
                //                BoundingBoxD newClusterAABB = aabb.Include(aabb.Center + velocityDir * IdealClusterSize / 2);

                originalAABB.InflateToMinimum(IdealClusterSize);

                System.Diagnostics.Debug.Assert(m_clusters.Contains(objectData.Cluster));

                var newContainmentType = objectData.Cluster.AABB.Contains(extendedAABB);
                if (newContainmentType != ContainmentType.Contains && !SingleCluster.HasValue)
                {
                    if (newContainmentType == ContainmentType.Disjoint)
                    { //Probably caused by teleport
                        m_clusterTree.OverlapAllBoundingBox(ref extendedAABB, m_returnedClusters);
                        if ((m_returnedClusters.Count == 1) && (m_returnedClusters[0].AABB.Contains(extendedAABB) == ContainmentType.Contains))
                        { //Just move object from one cluster to another
                            var oldCluster = objectData.Cluster;
                            RemoveObjectFromCluster(objectData, false);
                            if (oldCluster.Objects.Count == 0)
                            {
                                RemoveCluster(oldCluster);
                            }

                            AddObjectToCluster(m_returnedClusters[0], objectData.Id, false);
                        }
                        else
                        {
                            ReorderClusters(originalAABB.Include(oldAABB), id);
                        }
                    }
                    else
                    {
                        ReorderClusters(originalAABB.Include(oldAABB), id);
                    }
                }

                System.Diagnostics.Debug.Assert(m_objectsData[id].Cluster.AABB.Contains(objectData.AABB) == ContainmentType.Contains || SingleCluster.HasValue, "Inconsistency in clusters");
            }

            //foreach (var ob in m_objectsData)
            //{
            //    if (ob.Value.ActivationHandler.IsStatic && ob.Value.Cluster != null)
            //        System.Diagnostics.Debug.Assert(ob.Value.Cluster.AABB.Contains(ob.Value.AABB) != ContainmentType.Disjoint, "Inconsistency in clusters");
            //    else
            //      if (!ob.Value.ActivationHandler.IsStatic)
            //        System.Diagnostics.Debug.Assert(ob.Value.Cluster.AABB.Contains(ob.Value.AABB) == ContainmentType.Contains, "Inconsistency in clusters");
            //}
        }
Beispiel #29
0
        public void TestPath(Vector3D destination, bool landing)
        {
            if (m_navSet.Settings_Current.DestinationChanged || m_prevMover != m_navSet.Settings_Current.NavigatorMover)
            {
                m_logger.debugLog("new destination: " + destination, Logger.severity.INFO);
                m_navSet.Settings_Task_NavWay.DestinationChanged = false;
                m_prevMover = m_navSet.Settings_Current.NavigatorMover;
                m_runId++;
                m_pathLow.Clear();
                ClearAltPath();
                m_pathState = PathState.Not_Running;
                m_planetCheckDest.Stop();
                m_planetCheckSpeed.Stop();
            }
            //else
            //	m_logger.debugLog("destination unchanged", "TestPath()");

            if (Globals.UpdateCount < m_nextRunPath)
            {
                return;
            }
            m_nextRunPath = Globals.UpdateCount + 10ul;

            if (m_pathLow.Count != 0)
            {
                m_logger.debugLog("path low is running");
                return;
            }

            m_navBlock        = m_navSet.Settings_Current.NavigationBlock;
            m_destination     = destination;
            m_ignoreAsteroid  = m_navSet.Settings_Current.IgnoreAsteroid;
            m_landing         = landing;
            m_canChangeCourse = m_navSet.Settings_Current.PathfinderCanChangeCourse;
            MyEntity destEntity = m_navSet.Settings_Current.DestinationEntity as MyEntity;

            m_logger.debugLog("DestinationEntity: " + m_navSet.Settings_Current.DestinationEntity.getBestName());
            byte runId = m_runId;

            const float minimumDistance = 100f;
            const float minDistSquared  = minimumDistance * minimumDistance;
            const float seconds         = 10f;
            const float distOverSeconds = minimumDistance / seconds;

            Vector3 displacement    = destination - m_navBlock.WorldPosition;
            float   distanceSquared = displacement.LengthSquared();
            float   testDistance;
            Vector3 move_direction = m_grid.Physics.LinearVelocity;
            float   speedSquared   = move_direction.LengthSquared();

            if (distanceSquared > minDistSquared)
            {
                // only look ahead 10 s / 100 m
                testDistance = speedSquared < distOverSeconds ? minimumDistance : (float)Math.Sqrt(speedSquared) * seconds;
                if (testDistance * testDistance < distanceSquared)
                {
                    Vector3 direction = displacement / (float)Math.Sqrt(distanceSquared);
                    destination = m_navBlock.WorldPosition + testDistance * direction;
                    m_logger.debugLog("moved destination: " + destination + ", distance: " + testDistance + ", direction: " + direction);
                }
            }
            else
            {
                m_logger.debugLog("using actual destination: " + destination);
            }

            m_pathHigh.Enqueue(() => TestPath(destination, destEntity, runId, isAlternate: false, tryAlternates: true));
            if (m_ignoreAsteroid)
            {
                m_planetCheckDest.Stop();
            }
            else
            {
                m_planetCheckDest.Start(destination - m_navBlock.WorldPosition);
            }

            // given velocity and distance, calculate destination
            if (speedSquared > 1f)
            {
                Vector3D moveDest = m_navBlock.WorldPosition + move_direction * LookAheadSpeed_Seconds;
                m_pathHigh.Enqueue(() => TestPath(moveDest, null, runId, isAlternate: false, tryAlternates: false, slowDown: true));
                if (m_ignoreAsteroid)
                {
                    m_planetCheckSpeed.Stop();
                }
                else
                {
                    m_planetCheckSpeed.Start(moveDest - m_navBlock.WorldPosition);
                }
            }
            else
            {
                m_navSet.Settings_Task_NavWay.SpeedMaxRelative = float.MaxValue;
                m_navSet.Settings_Task_NavWay.SpeedTarget      = float.MaxValue;
            }

            RunItem();
        }
Beispiel #30
0
        protected Vector3 MoveFree(BackingBox TargetBox, Vector3 PositionDelta, bool ShouldHugTerrain = true)
        {
            if (PositionDelta.LengthSquared() == 0)
            {
                return(Vector3.One);
            }
            bool isOnGroundPreMove = false;

            if (ShouldHugTerrain && PositionDelta.Y == 0)
            {
                isOnGroundPreMove = IsBoxOnGround(TargetBox);
            }
            TargetBox.Move(PositionDelta);
            if (isOnGroundPreMove)
            {
                if (!IsBoxOnGround(TargetBox))
                {
                    // Ramps are only oriented north south for now
                    TargetBox.Move(new Vector3(0, -Math.Abs(PositionDelta.Z), 0));
                }
            }

            bool    hasCollided;
            Vector3 velocityAdjustments = Vector3.One;

            do
            {
                hasCollided = false;
                foreach (BackingBox box in boxes)
                {
                    if (!box.IsInteractive() || TargetBox == box || !TargetBox.B.DoesIntersect(box.B))
                    {
                        continue;
                    }
                    hasCollided = true;
                    float xIntersectRatio = -1;
                    float yIntersectRatio = -1;
                    float zIntersectRatio = -1;
                    if (PositionDelta.X > 0)
                    {
                        xIntersectRatio = Math.Abs(
                            TargetBox.GetFaceIntersectDistance(box, BackingBox.Face.Left) / PositionDelta.X
                            );
                    }
                    else if (PositionDelta.X < 0)
                    {
                        xIntersectRatio = Math.Abs(
                            TargetBox.GetFaceIntersectDistance(box, BackingBox.Face.Right) / PositionDelta.X
                            );
                    }
                    if (PositionDelta.Y > 0)
                    {
                        yIntersectRatio = Math.Abs(
                            TargetBox.GetFaceIntersectDistance(box, BackingBox.Face.Bottom) / PositionDelta.Y
                            );
                    }
                    else if (PositionDelta.Y < 0)
                    {
                        yIntersectRatio = Math.Abs(
                            TargetBox.GetFaceIntersectDistance(box, BackingBox.Face.Top) / PositionDelta.Y
                            );
                    }
                    else
                    {
                        yIntersectRatio = Math.Abs(
                            TargetBox.GetFaceIntersectDistance(box, BackingBox.Face.Top) / new Vector2(PositionDelta.X, PositionDelta.Z).Length()
                            );
                    }
                    if (PositionDelta.Z > 0)
                    {
                        zIntersectRatio = Math.Abs(
                            TargetBox.GetFaceIntersectDistance(box, BackingBox.Face.Back) / PositionDelta.Z
                            );
                    }
                    else if (PositionDelta.Z < 0)
                    {
                        zIntersectRatio = Math.Abs(
                            TargetBox.GetFaceIntersectDistance(box, BackingBox.Face.Front) / PositionDelta.Z
                            );
                    }
                    if (
                        (yIntersectRatio == -1 || (xIntersectRatio != -1 && xIntersectRatio <= yIntersectRatio)) &&
                        (zIntersectRatio == -1 || (xIntersectRatio != -1 && xIntersectRatio <= zIntersectRatio))
                        )
                    {
                        TargetBox.MatchFace(box, PositionDelta.X > 0 ? BackingBox.Face.Left : BackingBox.Face.Right);
                        velocityAdjustments.X = 0;
                    }
                    else if (
                        (xIntersectRatio == -1 || (yIntersectRatio != -1 && yIntersectRatio <= xIntersectRatio)) &&
                        (zIntersectRatio == -1 || (yIntersectRatio != -1 && yIntersectRatio <= zIntersectRatio))
                        )
                    {
                        if (box.B.isRamp)
                        {
                            if (PositionDelta.Z > 0)
                            {
                                velocityAdjustments.Z = 1f;
                            }
                            else if (PositionDelta.Z < 0)
                            {
                                velocityAdjustments.Z = 0.4f;
                            }
                        }

                        TargetBox.MatchFace(box, PositionDelta.Y > 0 ? BackingBox.Face.Bottom : BackingBox.Face.Top);
                        velocityAdjustments.Y = 0;
                    }
                    else if (
                        (xIntersectRatio == -1 || (zIntersectRatio != -1 && zIntersectRatio <= xIntersectRatio)) &&
                        (yIntersectRatio == -1 || (zIntersectRatio != -1 && zIntersectRatio <= yIntersectRatio))
                        )
                    {
                        TargetBox.MatchFace(box, PositionDelta.Z > 0 ? BackingBox.Face.Back : BackingBox.Face.Front);
                        velocityAdjustments.Z = 0;
                    }
                }
            } while (hasCollided);

            return(velocityAdjustments);
        }
Beispiel #31
0
	    public static void CalculateDiffAxisAngleQuaternion(ref Quaternion orn0,ref Quaternion orn1a,ref Vector3 axis,ref float angle)
	    {
            Quaternion orn1 = MathUtil.QuatFurthest(ref orn0,ref orn1a);
            Quaternion dorn = orn1 * MathUtil.QuaternionInverse(ref orn0);

		    ///floating point inaccuracy can lead to w component > 1..., which breaks 
		    dorn.Normalize();
		    angle = MathUtil.QuatAngle(ref dorn);
		    axis = new Vector3(dorn.X,dorn.Y,dorn.Z);

		    //check for axis length
		    float len = axis.LengthSquared();
		    if (len < MathUtil.SIMD_EPSILON*MathUtil.SIMD_EPSILON)
            {
			    axis = new Vector3(1f,0,0);
            }
		    else
            {
			    axis.Normalize();
            }
	    }
Beispiel #32
0
        /// <summary>
        /// Batch a new border image draw to the draw list.
        /// </summary>
        /// <param name="texture">The texture to use during the draw</param>
        /// <param name="worldMatrix">The world matrix of the element</param>
        /// <param name="sourceRectangle">The rectangle indicating the source region of the texture to use</param>
        /// <param name="elementSize">The size of the ui element</param>
        /// <param name="borderSize">The size of the borders in the texture in pixels (left/top/right/bottom)</param>
        /// <param name="color">The color to apply to the texture image.</param>
        /// <param name="depthBias">The depth bias of the ui element</param>
        /// <param name="imageOrientation">The rotation to apply on the image uv</param>
        /// <param name="swizzle">Swizzle mode indicating the swizzle use when sampling the texture in the shader</param>
        /// <param name="snapImage">Indicate if the image needs to be snapped or not</param>
        public void DrawImage(Texture texture, ref Matrix worldMatrix, ref RectangleF sourceRectangle, ref Vector3 elementSize, ref Vector4 borderSize,
                              ref Color color, int depthBias, ImageOrientation imageOrientation = ImageOrientation.AsIs, SwizzleMode swizzle = SwizzleMode.None, bool snapImage = false)
        {
            if (texture == null)
            {
                throw new ArgumentNullException(nameof(texture));
            }

            // Skip items with null size
            if (elementSize.LengthSquared() < MathUtil.ZeroTolerance)
            {
                return;
            }

            // Calculate the information needed to draw.
            var drawInfo = new UIImageDrawInfo
            {
                Source =
                {
                    X      = sourceRectangle.X / texture.ViewWidth,
                    Y      = sourceRectangle.Y / texture.ViewHeight,
                    Width  = sourceRectangle.Width / texture.ViewWidth,
                    Height = sourceRectangle.Height / texture.ViewHeight,
                },
                DepthBias  = depthBias,
                ColorScale = color,
                ColorAdd   = Color.Zero,
                Swizzle    = swizzle,
                SnapImage  = snapImage,
                Primitive  = borderSize == Vector4.Zero ? PrimitiveType.Rectangle : PrimitiveType.BorderRectangle,
                BorderSize = new Vector4(borderSize.X / sourceRectangle.Width, borderSize.Y / sourceRectangle.Height, borderSize.Z / sourceRectangle.Width, borderSize.W / sourceRectangle.Height),
            };

            var rotatedSize = imageOrientation == ImageOrientation.AsIs ? elementSize : new Vector3(elementSize.Y, elementSize.X, 0);

            drawInfo.VertexShift = new Vector4(borderSize.X / rotatedSize.X, borderSize.Y / rotatedSize.Y, 1f - borderSize.Z / rotatedSize.X, 1f - borderSize.W / rotatedSize.Y);

            var matrix = worldMatrix;

            matrix.M11 *= elementSize.X;
            matrix.M12 *= elementSize.X;
            matrix.M13 *= elementSize.X;
            matrix.M21 *= elementSize.Y;
            matrix.M22 *= elementSize.Y;
            matrix.M23 *= elementSize.Y;
            matrix.M31 *= elementSize.Z;
            matrix.M32 *= elementSize.Z;
            matrix.M33 *= elementSize.Z;

            Matrix worldViewProjection;

            Matrix.Multiply(ref matrix, ref viewProjectionMatrix, out worldViewProjection);
            drawInfo.UnitXWorld = worldViewProjection.Row1;
            drawInfo.UnitYWorld = worldViewProjection.Row2;

            // rotate origin and unit axis if need.
            var leftTopCorner = vector4LeftTop;

            if (imageOrientation == ImageOrientation.Rotated90)
            {
                var unitX = drawInfo.UnitXWorld;
                drawInfo.UnitXWorld = -drawInfo.UnitYWorld;
                drawInfo.UnitYWorld = unitX;
                leftTopCorner       = new Vector4(-0.5f, 0.5f, 0, 1);
            }
            Vector4.Transform(ref leftTopCorner, ref worldViewProjection, out drawInfo.LeftTopCornerWorld);

            var verticesPerElement = 4;
            var indicesPerElement  = 6;

            if (drawInfo.Primitive == PrimitiveType.BorderRectangle)
            {
                verticesPerElement = 16;
                indicesPerElement  = 54;
            }

            var elementInfo = new ElementInfo(verticesPerElement, indicesPerElement, in drawInfo, depthBias);

            Draw(texture, in elementInfo);
        }
Beispiel #33
0
		public bool AddSupportPoint(ref Vector3 newPoint)
		{
			int num = (BitsToIndices[simplexBits ^ 15] & 7) - 1;
			
			y[num] = newPoint;
			yLengthSq[num] = newPoint.LengthSquared();
			
			for (int num2 = BitsToIndices[simplexBits]; num2 != 0; num2 >>= 3)
			{
				int num3 = (num2 & 7) - 1;
				Vector3 vector = y[num3] - newPoint;
				edges[num3][num] = vector;
				edges[num][num3] = -vector;
				edgeLengthSq[num][num3] = (edgeLengthSq[num3][num] = vector.LengthSquared());
			}

			UpdateDeterminant(num);

			return UpdateSimplex(num);
		}
Beispiel #34
0
        public void Vector3LengthSquaredTest()
        {
            Vector2 a = new Vector2(1.0f, 2.0f);

            float z = 3.0f;

            Vector3 target = new Vector3(a, z);

            float expected = 14.0f;
            float actual;

            actual = target.LengthSquared();
            Assert.IsTrue(MathHelper.Equal(expected, actual), "Vector3.LengthSquared did not return the expected value.");
        }
Beispiel #35
0
        /// <summary>
        /// Do any necessary computations to prepare the constraint for this frame.
        /// </summary>
        /// <param name="dt">Simulation step length.</param>
        public override void Update(float dt)
        {
            basisA.rotationMatrix = connectionA.orientationMatrix;
            basisB.rotationMatrix = connectionB.orientationMatrix;
            basisA.ComputeWorldSpaceAxes();
            basisB.ComputeWorldSpaceAxes();

            Quaternion rotation;

            Toolbox.GetQuaternionBetweenNormalizedVectors(ref basisB.primaryAxis, ref basisA.primaryAxis, out rotation);

            //Transform b's 'Y' axis so that it is perpendicular with a's 'X' axis for measurement.
            Vector3 twistMeasureAxis;

            Vector3.Transform(ref basisB.xAxis, ref rotation, out twistMeasureAxis);

            //By dotting the measurement vector with a 2d plane's axes, we can get a local X and Y value.
            float y, x;

            Vector3.Dot(ref twistMeasureAxis, ref basisA.yAxis, out y);
            Vector3.Dot(ref twistMeasureAxis, ref basisA.xAxis, out x);
            var angle = (float)Math.Atan2(y, x);

            float distanceFromCurrent, distanceFromMaximum;

            if (IsAngleValid(angle, out distanceFromCurrent, out distanceFromMaximum))
            {
                isActiveInSolver   = false;
                accumulatedImpulse = 0;
                error         = 0;
                isLimitActive = false;
                return;
            }
            isLimitActive = true;

            //Compute the jacobian.
            if (error > 0)
            {
                Vector3.Add(ref basisA.primaryAxis, ref basisB.primaryAxis, out jacobianB);
                if (jacobianB.LengthSquared() < Toolbox.Epsilon)
                {
                    //A nasty singularity can show up if the axes are aligned perfectly.
                    //In a 'real' situation, this is impossible, so just ignore it.
                    isActiveInSolver = false;
                    return;
                }

                jacobianB.Normalize();
                jacobianA.X = -jacobianB.X;
                jacobianA.Y = -jacobianB.Y;
                jacobianA.Z = -jacobianB.Z;
            }
            else
            {
                //Reverse the jacobian so that the solver loop is easier.
                Vector3.Add(ref basisA.primaryAxis, ref basisB.primaryAxis, out jacobianA);
                if (jacobianA.LengthSquared() < Toolbox.Epsilon)
                {
                    //A nasty singularity can show up if the axes are aligned perfectly.
                    //In a 'real' situation, this is impossible, so just ignore it.
                    isActiveInSolver = false;
                    return;
                }

                jacobianA.Normalize();
                jacobianB.X = -jacobianA.X;
                jacobianB.Y = -jacobianA.Y;
                jacobianB.Z = -jacobianA.Z;
            }

            //****** VELOCITY BIAS ******//
            //Compute the correction velocity.
            error = ComputeAngleError(distanceFromCurrent, distanceFromMaximum);
            float errorReduction;

            springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);


            //biasVelocity = MathHelper.Clamp(-error * myCorrectionStrength / dt, -myMaxCorrectiveVelocity, myMaxCorrectiveVelocity);
            biasVelocity = MathHelper.Min(MathHelper.Max(0, Math.Abs(error) - margin) * errorReduction, maxCorrectiveVelocity);
            if (bounciness > 0)
            {
                float relativeVelocity;
                float dot;
                //Find the velocity contribution from each connection
                Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out relativeVelocity);
                Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out dot);
                relativeVelocity += dot;
                biasVelocity      = MathHelper.Max(biasVelocity, ComputeBounceVelocity(-relativeVelocity));
            }

            //The nice thing about this approach is that the jacobian entry doesn't flip.
            //Instead, the error can be negative due to the use of Atan2.
            //This is important for limits which have a unique high and low value.


            //****** EFFECTIVE MASS MATRIX ******//
            //Connection A's contribution to the mass matrix
            float   entryA;
            Vector3 transformedAxis;

            if (connectionA.isDynamic)
            {
                Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis);
                Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA);
            }
            else
            {
                entryA = 0;
            }

            //Connection B's contribution to the mass matrix
            float entryB;

            if (connectionB.isDynamic)
            {
                Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis);
                Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB);
            }
            else
            {
                entryB = 0;
            }

            //Compute the inverse mass matrix
            velocityToImpulse = 1 / (softness + entryA + entryB);
        }
Beispiel #36
0
		public eStatus Evaluate(GJK gjk,ref Vector3 guess)
		{
			sSimplex simplex=gjk.m_simplex;
			if((simplex.rank>1)&&gjk.EncloseOrigin())
			{
				/* Clean up				*/ 
				while(m_hull.Count > 0)
				{
					sFace	f = m_hull[0];
					Remove(m_hull,f);
					Append(m_stock,f);
				}

				m_status = eStatus.Valid;
				m_nextsv = 0;
				/* Orient simplex		*/ 
				if(GJK.Det(	simplex.c[0].w-simplex.c[3].w,
				           	simplex.c[1].w-simplex.c[3].w,
				           	simplex.c[2].w-simplex.c[3].w)<0)
				{
					SwapSv(simplex.c,0,1);
					SwapFloat(simplex.p,0,1);
				}
				/* Build initial hull	*/ 
				sFace[]	tetra ={NewFace(simplex.c[0],simplex.c[1],simplex.c[2],true),
				       	        NewFace(simplex.c[1],simplex.c[0],simplex.c[3],true),
				       	        NewFace(simplex.c[2],simplex.c[1],simplex.c[3],true),
				       	        NewFace(simplex.c[0],simplex.c[2],simplex.c[3],true)};
				if(m_hull.Count==4)
				{
					sFace best=FindBest();
					sFace outer = best;
					uint pass=0;
					uint iterations=0;
					Bind(tetra[0],0,tetra[1],0);
					Bind(tetra[0],1,tetra[2],0);
					Bind(tetra[0],2,tetra[3],0);
					Bind(tetra[1],1,tetra[3],2);
					Bind(tetra[1],2,tetra[2],1);
					Bind(tetra[2],2,tetra[3],1);
					m_status=eStatus.Valid;
					for (; iterations < GjkEpaSolver2.EPA_MAX_ITERATIONS; ++iterations)
					{
						if (m_nextsv < GjkEpaSolver2.EPA_MAX_VERTICES)
						{
							sHorizon horizon = new sHorizon() ;
							sSV	w = m_sv_store[m_nextsv++];
							bool valid = true;					
							best.pass =	(uint)(++pass);
							gjk.GetSupport(ref best.n,ref w);
							float wdist=Vector3.Dot(best.n,w.w)-best.d;
							if (wdist > GjkEpaSolver2.EPA_ACCURACY)
							{
								for(int j=0;(j<3)&&valid;++j)
								{
									valid&=Expand(	pass,w,
									              	best.f[j],best.e[j],
									              	horizon);
								}
								if(valid&&(horizon.nf>=3))
								{
									Bind(horizon.cf,1,horizon.ff,2);
									Remove(m_hull,best);
									Append(m_stock,best);
									best=FindBest();
									if (best.p >= outer.p)
									{
										outer = best;
									}
								} 
								else 
								{ 
									m_status=eStatus.InvalidHull;
									break; 
								}
							} 
							else 
							{ 
								m_status=eStatus.AccuraryReached;
								break; 
							}
						} 
						else 
						{ 
							m_status=eStatus.OutOfVertices;
							break; 
						}
					}
					Vector3	projection=outer.n*outer.d;
					m_normal	=	outer.n;
					m_depth		=	outer.d;
					m_result.rank	=	3;
					m_result.c[0]	=	outer.c[0];
					m_result.c[1]	=	outer.c[1];
					m_result.c[2]	=	outer.c[2];
					m_result.p[0]	=	Vector3.Cross(	outer.c[1].w-projection,
					             	 	              	outer.c[2].w-projection).Length();
					m_result.p[1] = Vector3.Cross(outer.c[2].w - projection,
					                              outer.c[0].w-projection).Length();
					m_result.p[2] = Vector3.Cross(outer.c[0].w - projection,
					                              outer.c[1].w-projection).Length();
					float sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
					m_result.p[0]	/=	sum;
					m_result.p[1]	/=	sum;
					m_result.p[2]	/=	sum;
					return(m_status);
				}
			}
			/* Fallback		*/ 
			m_status	=	eStatus.FallBack;
			m_normal	=	-guess;
			float nl=m_normal.LengthSquared();
			if(nl>0)
			{
				m_normal.Normalize();
			}
			else
			{
				m_normal = new Vector3(1,0,0);
			}

			m_depth	=	0;
			m_result.rank=1;
			m_result.c[0]=simplex.c[0];
			m_result.p[0]=1;	
			return(m_status);
		}
Beispiel #37
0
        public void UpdateBeforeSimulation()
        {
            if (m_gyrosChanged)
            {
                RecomputeGyroParameters();
            }

            if (m_maxOverrideForce == 0.0f)
            {
                if (MyDebugDrawSettings.DEBUG_DRAW_GYROS)
                {
                    MyRenderProxy.DebugDrawText2D(new Vector2(0.0f, 0.0f), "Old gyros", Color.White, 1.0f);
                }
                UpdateBeforeSimulationOld();
                return;
            }

            if (MyDebugDrawSettings.DEBUG_DRAW_GYROS)
            {
                MyRenderProxy.DebugDrawText2D(new Vector2(0.0f, 0.0f), "New gyros", Color.White, 1.0f);
            }

            /*if (m_grid.GridControllers.IsControlledByLocalPlayer || (!m_grid.GridControllers.IsControlledByAnyPlayer && Sync.IsServer) || (false && Sync.IsServer))
             * {*/
            // Not checking whether engines are running, since ControlTorque should be 0 when
            // engines are stopped (set by cockpit).
            if (PowerReceiver.SuppliedRatio > 0f && m_grid.Physics != null && m_grid.Physics.Enabled && !m_grid.Physics.RigidBody.IsFixed)
            {
                Matrix  invWorldRot          = m_grid.PositionComp.WorldMatrixInvScaled.GetOrientation();
                Matrix  worldRot             = m_grid.WorldMatrix.GetOrientation();
                Vector3 localAngularVelocity = Vector3.Transform(m_grid.Physics.AngularVelocity, ref invWorldRot);

                // CH: CAUTION: Don't try to use InertiaTensor, although it might be more intuitive in some cases.
                // I tried it and it's not an inverse of the InverseInertiaTensor! Only the InverseInertiaTensor seems to be correct!
                var     invTensor       = m_grid.Physics.RigidBody.InverseInertiaTensor;
                Vector3 invTensorVector = new Vector3(invTensor.M11, invTensor.M22, invTensor.M33);
                var     minInvTensor    = invTensorVector.Min();

                // Max rotation limiter
                float divider = Math.Max(1, minInvTensor * INV_TENSOR_MAX_LIMIT);

                // Calculate the velocity correction torque
                Vector3 correctionTorque    = Vector3.Zero;
                Vector3 desiredAcceleration = desiredAcceleration = (m_overrideTargetVelocity - localAngularVelocity) * MyEngineConstants.UPDATE_STEPS_PER_SECOND;

                // The correction is done by overridden gyros and by the remaining power of the controlled gyros
                // This is not entirely physically correct, but it feels good
                float correctionForce = m_maxOverrideForce + m_maxGyroForce * (1.0f - ControlTorque.Length());

                // This is to ensure that the correction is done uniformly in all axes
                desiredAcceleration = desiredAcceleration * Vector3.Normalize(invTensorVector);

                Vector3 desiredTorque           = desiredAcceleration / invTensorVector;
                float   framesToDesiredVelocity = desiredTorque.Length() / correctionForce;

                // If we are very close to the target velocity, just set it without applying the torque
                const float minimalBypassVelocity = 0.005f * 0.005f;
                if (framesToDesiredVelocity < 0.5f && m_overrideTargetVelocity.LengthSquared() < minimalBypassVelocity)
                {
                    m_grid.Physics.AngularVelocity = m_overrideTargetVelocity;
                    return;
                }

                if (!Vector3.IsZero(desiredAcceleration, 0.0001f))
                {
                    // The smoothing coefficient is here to avoid the slowdown stopping the ship abruptly, which doesn't look good
                    float smoothingCoeff = 1.0f - 0.8f / (float)Math.Exp(0.5f * framesToDesiredVelocity);
                    correctionTorque = Vector3.ClampToSphere(desiredTorque, correctionForce) * 0.95f * smoothingCoeff + desiredTorque * 0.05f * (1.0f - smoothingCoeff);

                    // A little black magic to make slowdown on large ships bigger
                    if (m_grid.GridSizeEnum == MyCubeSize.Large)
                    {
                        correctionTorque *= 2.0f;
                    }
                }

                Torque = (ControlTorque * m_maxGyroForce + correctionTorque) / divider;

                Torque *= PowerReceiver.SuppliedRatio;
                if (Torque.LengthSquared() > 0.0001f)
                {
                    // Manually apply torque and use minimal component of inverted inertia tensor to make rotate same in all axes
                    var delta = Torque * new Vector3(minInvTensor) * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
                    var newAngularVelocity = localAngularVelocity + delta;
                    m_grid.Physics.AngularVelocity = Vector3.Transform(newAngularVelocity, ref worldRot);
                }

                const float stoppingVelocitySq = 0.0003f * 0.0003f;
                if (ControlTorque == Vector3.Zero && m_overrideTargetVelocity == Vector3.Zero && m_grid.Physics.AngularVelocity != Vector3.Zero && m_grid.Physics.AngularVelocity.LengthSquared() < stoppingVelocitySq && m_grid.Physics.RigidBody.IsActive)
                {
                    m_grid.Physics.AngularVelocity = Vector3.Zero;
                }
                //}
            }
        }
        /// <summary>
        /// Rasterizes the line, producing a list of GlobalVoxelCoordinates's that intersect
        /// the line segment.
        /// </summary>
        /// <param name="Start">The start.</param>
        /// <param name="End">The end.</param>
        /// <returns></returns>
        public static IEnumerable <GlobalVoxelCoordinate> FastVoxelTraversal(Vector3 Start, Vector3 End)
        {
            if (L1(Start, End) < 1e-12 || HasNan(Start) || HasNan(End))
            {
                yield break;
            }

            // From "A Fast DestinationVoxel Traversal Algorithm for Ray Tracing"
            // by John Amanatides and Andrew Woo, 1987
            // <http://www.cse.yorku.ca/~amana/research/grid.pdf>
            // <http://citeseer.ist.psu.edu/viewdoc/summary?doi=10.1.1.42.3443>
            // Extensions to the described algorithm:
            //   • Imposed a distance limit.

            // The foundation of this algorithm is a parameterized representation of
            // the provided ray,
            //                    origin + t * direction,
            // except that t is not actually stored; rather, at any given point in the
            // traversal, we keep track of the *greater* t values which we would have
            // if we took a step sufficient to cross a cube boundary along that axis
            // (i.e. change the integer part of the coordinate) in the variables
            // tMaxX, tMaxY, and tMaxZ.

            Vector3 direction    = End - Start;
            var     cutoffLength = direction.LengthSquared() * 1.01f;

            direction.Normalize();

            // Direction to increment x,y,z when stepping.
            var stepX = Math.Sign(direction.X);
            var stepY = Math.Sign(direction.Y);
            var stepZ = Math.Sign(direction.Z);

            // See description above. The initial values depend on the fractional
            // part of the origin.
            var tMaxX = IntBound(Start.X, direction.X);
            var tMaxY = IntBound(Start.Y, direction.Y);
            var tMaxZ = IntBound(Start.Z, direction.Z);

            // The change in t when taking a step (always positive).
            var tDeltaX = stepX / direction.X;
            var tDeltaY = stepY / direction.Y;
            var tDeltaZ = stepZ / direction.Z;

            var endX = FloorInt(End.X);
            var endY = FloorInt(End.Y);
            var endZ = FloorInt(End.Z);

            while (true)
            {
                var r = GlobalVoxelCoordinate.FromVector3(Start);
                yield return(r);

                if (r.X == endX && r.Y == endY && r.Z == endZ)
                {
                    yield break;
                }
                if ((End - Start).LengthSquared() > cutoffLength)
                {
                    yield break;
                }

                // tMaxX stores the t-value at which we cross a cube boundary along the
                // X axis, and similarly for Y and Z. Therefore, choosing the least tMax
                // chooses the closest cube boundary. Only the first case of the four
                // has been commented in detail.
                if (tMaxX < tMaxY)
                {
                    if (tMaxX < tMaxZ)
                    {
                        // Update which cube we are now in.
                        Start.X += stepX;
                        // Adjust tMaxX to the next X-oriented boundary crossing.
                        tMaxX += tDeltaX;
                    }
                    else
                    {
                        Start.Z += stepZ;
                        tMaxZ   += tDeltaZ;
                    }
                }
                else
                {
                    if (tMaxY < tMaxZ)
                    {
                        Start.Y += stepY;
                        tMaxY   += tDeltaY;
                    }
                    else
                    {
                        // Identical to the second case, repeated for simplicity in
                        // the conditionals.
                        Start.Z += stepZ;
                        tMaxZ   += tDeltaZ;
                    }
                }
            }
        }
Beispiel #39
0
            public Triangle(Vector3 a, Vector3 b, Vector3 c)
            {
                A = a;
                Edge0 = b - a;
                Edge1 = c - a;

                PointOnPath = Vector3.Zero;
                Tangent = Vector3.Zero;

                // ReSharper disable once ImpureMethodCallOnReadonlyValueField
                var edge0LengthSquared = Edge0.LengthSquared();

                var edge0DotEdge1 = Vector3.Dot(Edge0, Edge1);
                var edge1LengthSquared = Vector3.Dot(Edge1, Edge1);

                Determinant = edge0LengthSquared * edge1LengthSquared - edge0DotEdge1 * edge0DotEdge1;
            }
Beispiel #40
0
        public void DoDamage(Creature performer, Body other, float bonus)
        {
            if (!String.IsNullOrEmpty(DiseaseToSpread))
            {
                var otherCreature = other.GetRoot().GetComponent <Creature>();
                if (otherCreature != null)
                {
                    var disease = DiseaseLibrary.GetDisease(DiseaseToSpread);
                    if (MathFunctions.RandEvent(disease.LikelihoodOfSpread))
                    {
                        otherCreature.AcquireDisease(DiseaseToSpread);
                    }
                }
            }

            var health = other.GetRoot().EnumerateAll().OfType <Health>().FirstOrDefault();

            if (health != null)
            {
                health.Damage(DamageAmount + bonus);
                var injury = DiseaseLibrary.GetRandomInjury();

                if (MathFunctions.RandEvent(injury.LikelihoodOfSpread))
                {
                    var creature = other.GetRoot().GetComponent <Creature>();
                    if (creature != null)
                    {
                        creature.AcquireDisease(injury.Name);
                    }
                }

                Vector3 knock = other.Position - performer.Physics.Position;
                knock.Normalize();
                knock *= 0.2f;
                if (other.AnimationQueue.Count == 0)
                {
                    other.AnimationQueue.Add(new KnockbackAnimation(0.15f, other.LocalTransform, knock));
                }
            }
            else
            {
                other.GetRoot().Die();
            }

            PlayNoise(other.GlobalTransform.Translation);
            if (HitParticles != "")
            {
                performer.Manager.World.ParticleManager.Trigger(HitParticles, other.LocalTransform.Translation, Color.White, 5);

                if (ShootLaser)
                {
                    performer.Manager.World.ParticleManager.TriggerRay(HitParticles, performer.AI.Position, other.LocalTransform.Translation);
                }
            }

            if (HitAnimation != null)
            {
                IndicatorManager.DrawIndicator(HitAnimation, other.BoundingBox.Center(), 10.0f, 1.0f, MathFunctions.RandVector2Circle(), Color.White, MathFunctions.Rand() > 0.5f);
            }

            Physics physics = other as Physics;

            if (physics != null)
            {
                Vector3 force = other.Position - performer.AI.Position;

                if (force.LengthSquared() > 0.01f)
                {
                    force.Normalize();
                    physics.ApplyForce(force * Knockback, 1.0f);
                }
            }
        }
Beispiel #41
0
 public bool AddSupportPoint(ref Vector3 newPoint)
 {
     int num = (Gjk.BitsToIndices[this.simplexBits ^ 15] & 7) - 1;
     this.y[num] = newPoint;
     this.yLengthSq[num] = newPoint.LengthSquared();
     for (int num2 = Gjk.BitsToIndices[this.simplexBits]; num2 != 0; num2 >>= 3)
     {
         int num3 = (num2 & 7) - 1;
         Vector3 vector = this.y[num3] - newPoint;
         this.edges[num3][num] = vector;
         this.edges[num][num3] = -vector;
         this.edgeLengthSq[num][num3] = (this.edgeLengthSq[num3][num] = vector.LengthSquared());
     }
     this.UpdateDeterminant(num);
     return this.UpdateSimplex(num);
 }
Beispiel #42
0
        ///<summary>
        /// Performs the frame's configuration step.
        ///</summary>
        ///<param name="dt">Timestep duration.</param>
        public override void Update(float dt)
        {
            //Transform the axes into world space.
            basis.rotationMatrix = connectionA.orientationMatrix;
            basis.ComputeWorldSpaceAxes();
            Matrix3X3.Transform(ref localTestAxis, ref connectionB.orientationMatrix, out worldTestAxis);

            //Compute the plane normals.
            Vector3 minPlaneNormal, maxPlaneNormal;
            //Rotate basisA y axis around the basisA primary axis.
            Matrix rotation;

            Matrix.CreateFromAxisAngle(ref basis.primaryAxis, minimumAngle + MathHelper.PiOver2, out rotation);
            Vector3.TransformNormal(ref basis.xAxis, ref rotation, out minPlaneNormal);
            Matrix.CreateFromAxisAngle(ref basis.primaryAxis, maximumAngle - MathHelper.PiOver2, out rotation);
            Vector3.TransformNormal(ref basis.xAxis, ref rotation, out maxPlaneNormal);

            //Compute the errors along the two normals.
            float planePositionMin, planePositionMax;

            Vector3.Dot(ref minPlaneNormal, ref worldTestAxis, out planePositionMin);
            Vector3.Dot(ref maxPlaneNormal, ref worldTestAxis, out planePositionMax);


            float span = GetDistanceFromMinimum(maximumAngle);

            //Early out and compute the determine the plane normal.
            if (span >= MathHelper.Pi)
            {
                if (planePositionMax > 0 || planePositionMin > 0)
                {
                    //It's in a perfectly valid configuration, so skip.
                    isActiveInSolver   = false;
                    minIsActive        = false;
                    maxIsActive        = false;
                    error              = Vector2.Zero;
                    accumulatedImpulse = Vector2.Zero;
                    isLimitActive      = false;
                    return;
                }

                if (planePositionMax > planePositionMin)
                {
                    //It's quicker to escape out to the max plane than the min plane.
                    error.X = 0;
                    error.Y = -planePositionMax;
                    accumulatedImpulse.X = 0;
                    minIsActive          = false;
                    maxIsActive          = true;
                }
                else
                {
                    //It's quicker to escape out to the min plane than the max plane.
                    error.X = -planePositionMin;
                    error.Y = 0;
                    accumulatedImpulse.Y = 0;
                    minIsActive          = true;
                    maxIsActive          = false;
                }
                //There's never a non-degenerate situation where having both planes active with a span
                //greater than pi is useful.
            }
            else
            {
                if (planePositionMax > 0 && planePositionMin > 0)
                {
                    //It's in a perfectly valid configuration, so skip.
                    isActiveInSolver   = false;
                    minIsActive        = false;
                    maxIsActive        = false;
                    error              = Vector2.Zero;
                    accumulatedImpulse = Vector2.Zero;
                    isLimitActive      = false;
                    return;
                }

                if (planePositionMin <= 0 && planePositionMax <= 0)
                {
                    //Escape upward.
                    //Activate both planes.
                    error.X     = -planePositionMin;
                    error.Y     = -planePositionMax;
                    minIsActive = true;
                    maxIsActive = true;
                }
                else if (planePositionMin <= 0)
                {
                    //It's quicker to escape out to the min plane than the max plane.
                    error.X = -planePositionMin;
                    error.Y = 0;
                    accumulatedImpulse.Y = 0;
                    minIsActive          = true;
                    maxIsActive          = false;
                }
                else
                {
                    //It's quicker to escape out to the max plane than the min plane.
                    error.X = 0;
                    error.Y = -planePositionMax;
                    accumulatedImpulse.X = 0;
                    minIsActive          = false;
                    maxIsActive          = true;
                }
            }
            isLimitActive = true;


            //****** VELOCITY BIAS ******//
            //Compute the correction velocity
            float errorReduction;

            springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness);

            //Compute the jacobians
            if (minIsActive)
            {
                Vector3.Cross(ref minPlaneNormal, ref worldTestAxis, out jacobianMinA);
                if (jacobianMinA.LengthSquared() < Toolbox.Epsilon)
                {
                    //The plane normal is aligned with the test axis.
                    //Use the basis's free axis.
                    jacobianMinA = basis.primaryAxis;
                }
                jacobianMinA.Normalize();
                jacobianMinB.X = -jacobianMinA.X;
                jacobianMinB.Y = -jacobianMinA.Y;
                jacobianMinB.Z = -jacobianMinA.Z;
            }
            if (maxIsActive)
            {
                Vector3.Cross(ref maxPlaneNormal, ref worldTestAxis, out jacobianMaxA);
                if (jacobianMaxA.LengthSquared() < Toolbox.Epsilon)
                {
                    //The plane normal is aligned with the test axis.
                    //Use the basis's free axis.
                    jacobianMaxA = basis.primaryAxis;
                }
                jacobianMaxA.Normalize();
                jacobianMaxB.X = -jacobianMaxA.X;
                jacobianMaxB.Y = -jacobianMaxA.Y;
                jacobianMaxB.Z = -jacobianMaxA.Z;
            }

            //Error is always positive
            if (minIsActive)
            {
                biasVelocity.X = MathHelper.Min(MathHelper.Max(0, error.X - margin) * errorReduction, maxCorrectiveVelocity);
                if (bounciness > 0)
                {
                    float relativeVelocity;
                    float dot;
                    //Find the velocity contribution from each connection
                    Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMinA, out relativeVelocity);
                    Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMinB, out dot);
                    relativeVelocity += dot;
                    if (-relativeVelocity > bounceVelocityThreshold)
                    {
                        biasVelocity.X = MathHelper.Max(biasVelocity.X, -bounciness * relativeVelocity);
                    }
                }
            }
            if (maxIsActive)
            {
                biasVelocity.Y = MathHelper.Min(MathHelper.Max(0, error.Y - margin) * errorReduction, maxCorrectiveVelocity);
                if (bounciness > 0)
                {
                    //Find the velocity contribution from each connection
                    if (maxIsActive)
                    {
                        float relativeVelocity;
                        Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMaxA, out relativeVelocity);
                        float dot;
                        Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMaxB, out dot);
                        relativeVelocity += dot;
                        if (-relativeVelocity > bounceVelocityThreshold)
                        {
                            biasVelocity.Y = MathHelper.Max(biasVelocity.Y, -bounciness * relativeVelocity);
                        }
                    }
                }
            }


            //****** EFFECTIVE MASS MATRIX ******//
            //Connection A's contribution to the mass matrix
            float   minEntryA, minEntryB;
            float   maxEntryA, maxEntryB;
            Vector3 transformedAxis;

            if (connectionA.isDynamic)
            {
                if (minIsActive)
                {
                    Matrix3X3.Transform(ref jacobianMinA, ref connectionA.inertiaTensorInverse, out transformedAxis);
                    Vector3.Dot(ref transformedAxis, ref jacobianMinA, out minEntryA);
                }
                else
                {
                    minEntryA = 0;
                }
                if (maxIsActive)
                {
                    Matrix3X3.Transform(ref jacobianMaxA, ref connectionA.inertiaTensorInverse, out transformedAxis);
                    Vector3.Dot(ref transformedAxis, ref jacobianMaxA, out maxEntryA);
                }
                else
                {
                    maxEntryA = 0;
                }
            }
            else
            {
                minEntryA = 0;
                maxEntryA = 0;
            }
            //Connection B's contribution to the mass matrix
            if (connectionB.isDynamic)
            {
                if (minIsActive)
                {
                    Matrix3X3.Transform(ref jacobianMinB, ref connectionB.inertiaTensorInverse, out transformedAxis);
                    Vector3.Dot(ref transformedAxis, ref jacobianMinB, out minEntryB);
                }
                else
                {
                    minEntryB = 0;
                }
                if (maxIsActive)
                {
                    Matrix3X3.Transform(ref jacobianMaxB, ref connectionB.inertiaTensorInverse, out transformedAxis);
                    Vector3.Dot(ref transformedAxis, ref jacobianMaxB, out maxEntryB);
                }
                else
                {
                    maxEntryB = 0;
                }
            }
            else
            {
                minEntryB = 0;
                maxEntryB = 0;
            }
            //Compute the inverse mass matrix
            //Notice that the mass matrix isn't linked, it's two separate ones.
            velocityToImpulse.X = 1 / (softness + minEntryA + minEntryB);
            velocityToImpulse.Y = 1 / (softness + maxEntryA + maxEntryB);
        }