Add() public method

public Add ( Vector3 v ) : Vector3
v Vector3
return Vector3
Esempio n. 1
0
 private static Vector3 GetAnchorGuess(Entity connectionA, Entity connectionB)
 {
     var anchor = new Vector3();
     if (connectionA != null)
         anchor.Add( ref connectionA.position, out anchor );
     if (connectionB != null)
         anchor.Add( ref connectionB.position, out anchor );
     if (connectionA != null && connectionB != null)
         anchor.Mult( 0.5f, out anchor );
     return anchor;
 }
Esempio n. 2
0
        public void When_Adding_Two_Vectors_Vector_With_Result_Is_Returned()
        {
            // Arrange
            Vector3 vectorOne = new Vector3(1.0, 1.0, 1.0);
            Vector3 vectorTwo = new Vector3(2.0, 3.0, 4.0);

            // Act
            Vector3 result = vectorOne.Add(vectorTwo);

            // Assert
            Assert.AreEqual(3, result.X);
            Assert.AreEqual(4, result.Y);
            Assert.AreEqual(5, result.Z);
        }
Esempio n. 3
0
        private bool HitTest(Vector3 origin, Vector3 direction, float maxDistance, float stepSize, out Vector3 hitPos, out float iterations)
        {
            Vector3 step = direction.Mul(stepSize);
            hitPos = origin.Add(step);
            iterations = 0;

            while (hitPos.Sub(ref origin).Len() < maxDistance)
            {
                iterations = fractal.HitTest(hitPos);
                if (iterations >= threshold)
                {
                    return true;
                }

                hitPos.AddAssign(ref step);
            }

            return false;
        }
Esempio n. 4
0
        /// <summary>
        /// Adjusts the contrast of a pixel.
        /// </summary>
        /// <param name="argb">The ARGB pixel to adjust.</param>
        /// <param name="scale">The value to scale the contrast by.</param>
        /// <returns>The adjusted ARGB pixel.</returns>
        public static int AdjustContrast(int argb, float scale)
        {
            int a = (argb >> 24) & 0xFF;
            int r = (argb >> 16) & 0xFF;
            int g = (argb >> 8) & 0xFF;
            int b = (argb) & 0xFF;

            Vector3 res = new Vector3(r, g, b);
            res.Multiply(1 / 255.0f);
            res.Subtract(0.5f);
            res.Multiply(scale);
            res.Add(0.5f);
            res.Multiply(255.0f);
            res.Clamp(0, 255);

            r = (int)res.X;
            g = (int)res.Y;
            b = (int)res.Z;
            return (a << 24) | (r << 16) | (g << 8) | b;
        }
Esempio n. 5
0
        ///<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);
        }
Esempio n. 6
0
        void GetSamplingOrigin(ref BoundingBox entityBoundingBox, out Vector3 xSpacing, out Vector3 zSpacing, out float perColumnArea, out Vector3 origin)
        {
            //Compute spacing and increment informaiton.
            float   widthIncrement  = (entityBoundingBox.Max.X - entityBoundingBox.Min.X) / samplePointsPerDimension;
            float   lengthIncrement = (entityBoundingBox.Max.Z - entityBoundingBox.Min.Z) / samplePointsPerDimension;
            Vector3 right           = Toolbox.RightVector;// new Vector3(surfaceOrientationTranspose.M11, surfaceOrientationTranspose.M21, surfaceOrientationTranspose.M31);

            Vector3.Multiply(ref right, widthIncrement, out xSpacing);
            Vector3 backward = Toolbox.BackVector;// new Vector3(surfaceOrientationTranspose.M13, surfaceOrientationTranspose.M23, surfaceOrientationTranspose.M33);

            Vector3.Multiply(ref backward, lengthIncrement, out zSpacing);
            perColumnArea = widthIncrement * lengthIncrement;


            //Compute the origin.
            Vector3 minimum = entityBoundingBox.Min;
            //Matrix3X3.TransformTranspose(ref entityBoundingBox.Min, ref surfaceOrientationTranspose, out minimum);
            Vector3 offset;

            Vector3.Multiply(ref xSpacing, .5f, out offset);
            Vector3.Add(ref minimum, ref offset, out origin);
            Vector3.Multiply(ref zSpacing, .5f, out offset);
            Vector3.Add(ref origin, ref offset, out origin);


            //TODO: Could adjust the grid origin such that a ray always hits the deepest point.
            //The below code is a prototype of the idea, but has bugs.
            //var convexInfo = info as ConvexCollisionInformation;
            //if (convexInfo != null)
            //{
            //    Vector3 dir;
            //    Vector3.Negate(ref upVector, out dir);
            //    Vector3 extremePoint;
            //    convexInfo.Shape.GetExtremePoint(dir, ref convexInfo.worldTransform, out extremePoint);
            //    //Use extreme point to snap to grid.
            //    Vector3.Subtract(ref extremePoint, ref origin, out offset);
            //    float offsetX, offsetZ;
            //    Vector3.Dot(ref offset, ref right, out offsetX);
            //    Vector3.Dot(ref offset, ref backward, out offsetZ);
            //    offsetX %= widthIncrement;
            //    offsetZ %= lengthIncrement;

            //    if (offsetX > .5f * widthIncrement)
            //    {
            //        Vector3.Multiply(ref right, 1 - offsetX, out offset);
            //    }
            //    else
            //    {
            //        Vector3.Multiply(ref right, -offsetX, out offset);
            //    }

            //    if (offsetZ > .5f * lengthIncrement)
            //    {
            //        Vector3 temp;
            //        Vector3.Multiply(ref right, 1 - offsetZ, out temp);
            //        Vector3.Add(ref temp, ref offset, out offset);
            //    }
            //    else
            //    {
            //        Vector3 temp;
            //        Vector3.Multiply(ref right, -offsetZ, out temp);
            //        Vector3.Add(ref temp, ref offset, out offset);
            //    }

            //    Vector3.Add(ref origin, ref offset, out origin);


            //}
        }
Esempio n. 7
0
 /// <summary>
 /// Adds two instances.
 /// </summary>
 /// <param name="left">The left operand of the addition.</param>
 /// <param name="right">The right operand of the addition.</param>
 /// <param name="result">A new instance that is the result of the addition.</param>
 public static void Add(this Matrix3 left, Matrix3 right, out Matrix3 result)
 {
     Vector3.Add(ref left.Row0, ref right.Row0, out result.Row0);
     Vector3.Add(ref left.Row1, ref right.Row1, out result.Row1);
     Vector3.Add(ref left.Row2, ref right.Row2, out result.Row2);
 }
Esempio n. 8
0
        private static void ComputeInitialTetrahedron(RawList <Vector3> points, RawList <int> outsidePoints, RawList <int> triangleIndices, out Vector3 centroid)
        {
            //Find four points on the hull.
            //We'll start with using the x axis to identify two points on the hull.
            int     a, b, c, d;
            Vector3 direction;
            //Find the extreme points along the x axis.
            float minimumX = float.MaxValue, maximumX = -float.MaxValue;
            int   minimumXIndex = 0, maximumXIndex = 0;

            for (int i = 0; i < points.count; ++i)
            {
                var v = points.Elements[i];
                if (v.X > maximumX)
                {
                    maximumX      = v.X;
                    maximumXIndex = i;
                }
                else if (v.X < minimumX)
                {
                    minimumX      = v.X;
                    minimumXIndex = i;
                }
            }
            a = minimumXIndex;
            b = maximumXIndex;
            //Check for redundancies..
            if (a == b)
            {
                throw new Exception("Point set is degenerate; convex hulls must have volume.");
            }

            //Now, use a second axis perpendicular to the two points we found.
            Vector3 ab;

            Vector3.Subtract(ref points.Elements[b], ref points.Elements[a], out ab);
            Vector3.Cross(ref ab, ref Toolbox.UpVector, out direction);
            if (direction.LengthSquared() < Toolbox.Epsilon)
            {
                Vector3.Cross(ref ab, ref Toolbox.RightVector, out direction);
            }
            float minimumDot, maximumDot;
            int   minimumIndex, maximumIndex;

            GetExtremePoints(ref direction, points, out maximumDot, out minimumDot, out maximumIndex, out minimumIndex);
            //Compare the location of the extreme points to the location of the axis.
            float dot;

            Vector3.Dot(ref direction, ref points.Elements[a], out dot);
            //Use the point further from the axis.
            if (Math.Abs(dot - minimumDot) > Math.Abs(dot - maximumDot))
            {
                //In this case, we should use the minimum index.
                c = minimumIndex;
            }
            else
            {
                //In this case, we should use the maximum index.
                c = maximumIndex;
            }

            //Check for redundancies..
            if (a == c || b == c)
            {
                throw new Exception("Point set is degenerate; convex hulls must have volume.");
            }

            //Use a third axis perpendicular to the plane defined by the three unique points a, b, and c.
            Vector3 ac;

            Vector3.Subtract(ref points.Elements[c], ref points.Elements[a], out ac);
            Vector3.Cross(ref ab, ref ac, out direction);

            GetExtremePoints(ref direction, points, out maximumDot, out minimumDot, out maximumIndex, out minimumIndex);
            //Compare the location of the extreme points to the location of the plane.
            Vector3.Dot(ref direction, ref points.Elements[a], out dot);
            //Use the point further from the plane.
            if (Math.Abs(dot - minimumDot) > Math.Abs(dot - maximumDot))
            {
                //In this case, we should use the minimum index.
                d = minimumIndex;
            }
            else
            {
                //In this case, we should use the maximum index.
                d = maximumIndex;
            }

            //Check for redundancies..
            if (a == d || b == d || c == d)
            {
                throw new Exception("Point set is degenerate; convex hulls must have volume.");
            }

            //Add the triangles.
            triangleIndices.Add(a);
            triangleIndices.Add(b);
            triangleIndices.Add(c);

            triangleIndices.Add(a);
            triangleIndices.Add(b);
            triangleIndices.Add(d);

            triangleIndices.Add(a);
            triangleIndices.Add(c);
            triangleIndices.Add(d);

            triangleIndices.Add(b);
            triangleIndices.Add(c);
            triangleIndices.Add(d);

            //The centroid is guaranteed to be within the convex hull.  It will be used to verify the windings of triangles throughout the hull process.
            Vector3.Add(ref points.Elements[a], ref points.Elements[b], out centroid);
            Vector3.Add(ref centroid, ref points.Elements[c], out centroid);
            Vector3.Add(ref centroid, ref points.Elements[d], out centroid);
            Vector3.Multiply(ref centroid, 0.25f, out centroid);

            for (int i = 0; i < triangleIndices.count; i += 3)
            {
                var vA = points.Elements[triangleIndices.Elements[i]];
                var vB = points.Elements[triangleIndices.Elements[i + 1]];
                var vC = points.Elements[triangleIndices.Elements[i + 2]];

                //Check the signed volume of a parallelepiped with the edges of this triangle and the centroid.
                Vector3 cross;
                Vector3.Subtract(ref vB, ref vA, out ab);
                Vector3.Subtract(ref vC, ref vA, out ac);
                Vector3.Cross(ref ac, ref ab, out cross);
                Vector3 offset;
                Vector3.Subtract(ref vA, ref centroid, out offset);
                float volume;
                Vector3.Dot(ref offset, ref cross, out volume);
                //This volume/cross product could also be used to check for degeneracy, but we already tested for that.
                if (Math.Abs(volume) < Toolbox.BigEpsilon)
                {
                    throw new Exception("Point set is degenerate; convex hulls must have volume.");
                }
                if (volume < 0)
                {
                    //If the signed volume is negative, that means the triangle's winding is opposite of what we want.
                    //Flip it around!
                    var temp = triangleIndices.Elements[i];
                    triangleIndices.Elements[i]     = triangleIndices.Elements[i + 1];
                    triangleIndices.Elements[i + 1] = temp;
                }
            }
        }
Esempio n. 9
0
        private static bool TryTetrahedronTriangle(ref Vector3 A, ref Vector3 B, ref Vector3 C,
                                                   ref Vector3 A1, ref Vector3 B1, ref Vector3 C1,
                                                   ref Vector3 A2, ref Vector3 B2, ref Vector3 C2,
                                                   float errorTolerance,
                                                   ref Vector3 otherPoint, out PairSimplex simplex, out Vector3 point)
        {
            //Note that there may be some extra terms that can be removed from this process.
            //Some conditions could use less parameters, since it is known that the origin
            //is not 'behind' BC or AC.

            simplex = new PairSimplex();
            point   = new Vector3();


            Vector3 ab, ac;

            Vector3.Subtract(ref B, ref A, out ab);
            Vector3.Subtract(ref C, ref A, out ac);
            Vector3 normal;

            Vector3.Cross(ref ab, ref ac, out normal);
            float   AdotN, ADdotN;
            Vector3 AD;

            Vector3.Subtract(ref otherPoint, ref A, out AD);
            Vector3.Dot(ref A, ref normal, out AdotN);
            Vector3.Dot(ref AD, ref normal, out ADdotN);

            //If (-A * N) * (AD * N) < 0, D and O are on opposite sides of the triangle.
            if (AdotN * ADdotN >= -Toolbox.Epsilon * errorTolerance)
            {
                //The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector,
                //just use -A.
                //Same for B->, C->P...

                //Check to see if it's outside A.
                //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside A.
                float AdotAB, AdotAC;
                Vector3.Dot(ref ab, ref A, out AdotAB);
                Vector3.Dot(ref ac, ref A, out AdotAC);
                AdotAB = -AdotAB;
                AdotAC = -AdotAC;
                if (AdotAC <= 0f && AdotAB <= 0)
                {
                    //It is A!
                    simplex.State      = SimplexState.Point;
                    simplex.A          = A;
                    simplex.U          = 1;
                    simplex.SimplexA.A = A1;
                    simplex.SimplexB.A = A2;
                    point = A;
                    return(true);
                }

                //Check to see if it's outside B.
                //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside B.
                float BdotAB, BdotAC;
                Vector3.Dot(ref ab, ref B, out BdotAB);
                Vector3.Dot(ref ac, ref B, out BdotAC);
                BdotAB = -BdotAB;
                BdotAC = -BdotAC;
                if (BdotAB >= 0f && BdotAC <= BdotAB)
                {
                    //It is B!
                    simplex.State      = SimplexState.Point;
                    simplex.A          = B;
                    simplex.U          = 1;
                    simplex.SimplexA.A = B1;
                    simplex.SimplexB.A = B2;
                    point = B;
                    return(true);
                }

                //Check to see if it's outside AB.
                float vc = AdotAB * BdotAC - BdotAB * AdotAC;
                if (vc <= 0 && AdotAB > 0 && BdotAB < 0) //Note > and < instead of => <=; avoids possibly division by zero
                {
                    simplex.State      = SimplexState.Segment;
                    simplex.V          = AdotAB / (AdotAB - BdotAB);
                    simplex.U          = 1 - simplex.V;
                    simplex.A          = A;
                    simplex.B          = B;
                    simplex.SimplexA.A = A1;
                    simplex.SimplexB.A = A2;
                    simplex.SimplexA.B = B1;
                    simplex.SimplexB.B = B2;

                    Vector3.Multiply(ref ab, simplex.V, out point);
                    Vector3.Add(ref point, ref A, out point);
                    return(true);
                }

                //Check to see if it's outside C.
                //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C.
                float CdotAB, CdotAC;
                Vector3.Dot(ref ab, ref C, out CdotAB);
                Vector3.Dot(ref ac, ref C, out CdotAC);
                CdotAB = -CdotAB;
                CdotAC = -CdotAC;
                if (CdotAC >= 0f && CdotAB <= CdotAC)
                {
                    //It is C!
                    simplex.State      = SimplexState.Point;
                    simplex.A          = C;
                    simplex.U          = 1;
                    simplex.SimplexA.A = C1;
                    simplex.SimplexB.A = C2;
                    point = C;
                    return(true);
                }

                //Check if it's outside AC.
                //float AdotAB, AdotAC;
                //Vector3.Dot(ref ab, ref A, out AdotAB);
                //Vector3.Dot(ref ac, ref A, out AdotAC);
                //AdotAB = -AdotAB;
                //AdotAC = -AdotAC;
                float vb = CdotAB * AdotAC - AdotAB * CdotAC;
                if (vb <= 0f && AdotAC > 0f && CdotAC < 0f) //Note > instead of >= and < instead of <=; prevents bad denominator
                {
                    simplex.State      = SimplexState.Segment;
                    simplex.A          = A;
                    simplex.B          = C;
                    simplex.SimplexA.A = A1;
                    simplex.SimplexA.B = C1;
                    simplex.SimplexB.A = A2;
                    simplex.SimplexB.B = C2;
                    simplex.V          = AdotAC / (AdotAC - CdotAC);
                    simplex.U          = 1 - simplex.V;
                    Vector3.Multiply(ref ac, simplex.V, out point);
                    Vector3.Add(ref point, ref A, out point);
                    return(true);
                }

                //Check if it's outside BC.
                //float BdotAB, BdotAC;
                //Vector3.Dot(ref ab, ref B, out BdotAB);
                //Vector3.Dot(ref ac, ref B, out BdotAC);
                //BdotAB = -BdotAB;
                //BdotAC = -BdotAC;
                float va = BdotAB * CdotAC - CdotAB * BdotAC;
                float d3d4;
                float d6d5;
                if (va <= 0f && (d3d4 = BdotAC - BdotAB) > 0f && (d6d5 = CdotAB - CdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator
                {
                    simplex.State      = SimplexState.Segment;
                    simplex.A          = B;
                    simplex.B          = C;
                    simplex.SimplexA.A = B1;
                    simplex.SimplexA.B = C1;
                    simplex.SimplexB.A = B2;
                    simplex.SimplexB.B = C2;
                    simplex.V          = d3d4 / (d3d4 + d6d5);
                    simplex.U          = 1 - simplex.V;

                    Vector3 bc;
                    Vector3.Subtract(ref C, ref B, out bc);
                    Vector3.Multiply(ref bc, simplex.V, out point);
                    Vector3.Add(ref point, ref B, out point);
                    return(true);
                }


                //On the face of the triangle.
                simplex.A          = A;
                simplex.B          = B;
                simplex.C          = C;
                simplex.SimplexA.A = A1;
                simplex.SimplexA.B = B1;
                simplex.SimplexA.C = C1;
                simplex.SimplexB.A = A2;
                simplex.SimplexB.B = B2;
                simplex.SimplexB.C = C2;
                simplex.State      = SimplexState.Triangle;
                float denom = 1f / (va + vb + vc);
                simplex.W = vc * denom;
                simplex.V = vb * denom;
                simplex.U = 1 - simplex.V - simplex.W;
                Vector3.Multiply(ref ab, simplex.V, out point);
                Vector3 acw;
                Vector3.Multiply(ref ac, simplex.W, out acw);
                Vector3.Add(ref A, ref point, out point);
                Vector3.Add(ref point, ref acw, out point);
                return(true);
            }
            return(false);
        }
Esempio n. 10
0
        ///<summary>
        /// Constructs a new pair simplex.
        ///</summary>
        ///<param name="cachedSimplex">Cached simplex to use to warmstart the simplex.</param>
        ///<param name="localTransformB">Transform of shape B in the local space of A.</param>
        public PairSimplex(ref CachedSimplex cachedSimplex, ref RigidTransform localTransformB)
        {
            //NOTE:
            //USING A CACHED SIMPLEX INVALIDATES ASSUMPTIONS THAT ALLOW SIMPLEX CASES TO BE IGNORED!
            //To get those assumptions back, either DO NOT USE CACHED SIMPLEXES, or
            //VERIFY THE SIMPLEXES.
            //-A point requires no verification.
            //-A segment needs verification that the origin is in front of A in the direction of B.
            //-A triangle needs verification that the origin is within the edge planes and in the direction of C.
            //-A tetrahedron needs verification that the origin is within the edge planes of triangle ABC and is in the direction of D.

            //This simplex implementation will not ignore any cases, so we can warm start safely with one problem.
            //Due to relative movement, the simplex may become degenerate.  Edges could become points, etc.
            //Some protections are built into the simplex cases, but keep an eye out for issues.
            //Most dangerous degeneracy seen so far is tetrahedron.  It fails to find any points on opposing sides due to numerical problems and returns intersection.


            previousDistanceToClosest = float.MaxValue;
            errorTolerance            = 0;
            LocalTransformB           = localTransformB;

            //Transform the SimplexB into the working space of the simplex and compute the working space simplex.
            State    = cachedSimplex.State;
            SimplexA = cachedSimplex.LocalSimplexA;
            SimplexB = new ContributingShapeSimplex();
            U        = 0;
            V        = 0;
            W        = 0;
            switch (State)
            {
            case SimplexState.Point:
                Quaternion.Transform(ref cachedSimplex.LocalSimplexB.A, ref LocalTransformB.Orientation, out SimplexB.A);
                Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);

                Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
                B = new Vector3();
                C = new Vector3();
                D = new Vector3();
                break;

            case SimplexState.Segment:
                Matrix3x3 transform;
                Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B);
                Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
                Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B);

                Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
                Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B);
                C = new Vector3();
                D = new Vector3();

                ////Test for degeneracy.
                //float edgeLengthAB;
                //Vector3.DistanceSquared(ref A, ref B, out edgeLengthAB);
                //if (edgeLengthAB < Toolbox.Epsilon)
                //    State = SimplexState.Point;

                break;

            case SimplexState.Triangle:
                Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.C, ref transform, out SimplexB.C);
                Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
                Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B);
                Vector3.Add(ref SimplexB.C, ref LocalTransformB.Position, out SimplexB.C);

                Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
                Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B);
                Vector3.Subtract(ref SimplexA.C, ref SimplexB.C, out C);
                D = new Vector3();

                ////Test for degeneracy.
                //Vector3 AB, AC;
                //Vector3.Subtract(ref B, ref A, out AB);
                //Vector3.Subtract(ref C, ref A, out AC);
                //Vector3 cross;
                //Vector3.Cross(ref AB, ref AC, out cross);
                ////If the area is small compared to a tolerance (adjusted by the partial perimeter), it's degenerate.
                //if (cross.LengthSquared() < Toolbox.BigEpsilon * (AB.LengthSquared() + AC.LengthSquared()))
                //    State = SimplexState.Point;


                break;

            case SimplexState.Tetrahedron:
                Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.C, ref transform, out SimplexB.C);
                Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.D, ref transform, out SimplexB.D);
                Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A);
                Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B);
                Vector3.Add(ref SimplexB.C, ref LocalTransformB.Position, out SimplexB.C);
                Vector3.Add(ref SimplexB.D, ref LocalTransformB.Position, out SimplexB.D);

                Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A);
                Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B);
                Vector3.Subtract(ref SimplexA.C, ref SimplexB.C, out C);
                Vector3.Subtract(ref SimplexA.D, ref SimplexB.D, out D);

                ////Test for degeneracy.
                //Vector3 AD;
                //Vector3.Subtract(ref B, ref A, out AB);
                //Vector3.Subtract(ref C, ref A, out AC);
                //Vector3.Subtract(ref D, ref A, out AD);
                //Vector3.Cross(ref AB, ref AC, out cross);
                //float volume;
                //Vector3.Dot(ref cross, ref AD, out volume);

                ////Volume is small compared to partial 'perimeter.'
                //if (volume < Toolbox.BigEpsilon * (AB.LengthSquared() + AC.LengthSquared() + AD.LengthSquared()))
                //    State = SimplexState.Point;
                break;

            default:
                A = new Vector3();
                B = new Vector3();
                C = new Vector3();
                D = new Vector3();
                break;
            }
        }
Esempio n. 11
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;
        }
Esempio n. 12
0
        private void GetBoundingBox(ref Matrix3x3 o, out BoundingBox boundingBox)
        {
#if !WINDOWS
            boundingBox = new BoundingBox();
#endif
            //Sample the local directions from the matrix, implicitly transposed.
            var rightDirection = new Vector3(o.M11, o.M21, o.M31);
            var upDirection    = new Vector3(o.M12, o.M22, o.M32);
            var backDirection  = new Vector3(o.M13, o.M23, o.M33);

            int   right = 0, left = 0, up = 0, down = 0, backward = 0, forward = 0;
            float minX = float.MaxValue, maxX = -float.MaxValue, minY = float.MaxValue, maxY = -float.MaxValue, minZ = float.MaxValue, maxZ = -float.MaxValue;

            for (int i = 0; i < hullVertices.Count; i++)
            {
                float dotX, dotY, dotZ;
                Vector3.Dot(ref rightDirection, ref hullVertices.Elements[i], out dotX);
                Vector3.Dot(ref upDirection, ref hullVertices.Elements[i], out dotY);
                Vector3.Dot(ref backDirection, ref hullVertices.Elements[i], out dotZ);
                if (dotX < minX)
                {
                    minX = dotX;
                    left = i;
                }
                if (dotX > maxX)
                {
                    maxX  = dotX;
                    right = i;
                }

                if (dotY < minY)
                {
                    minY = dotY;
                    down = i;
                }
                if (dotY > maxY)
                {
                    maxY = dotY;
                    up   = i;
                }

                if (dotZ < minZ)
                {
                    minZ    = dotZ;
                    forward = i;
                }
                if (dotZ > maxZ)
                {
                    maxZ     = dotZ;
                    backward = i;
                }
            }

            //Incorporate the collision margin.
            Vector3.Multiply(ref rightDirection, meshCollisionMargin / (float)Math.Sqrt(rightDirection.Length()), out rightDirection);
            Vector3.Multiply(ref upDirection, meshCollisionMargin / (float)Math.Sqrt(upDirection.Length()), out upDirection);
            Vector3.Multiply(ref backDirection, meshCollisionMargin / (float)Math.Sqrt(backDirection.Length()), out backDirection);

            var rightElement    = hullVertices.Elements[right];
            var leftElement     = hullVertices.Elements[left];
            var upElement       = hullVertices.Elements[up];
            var downElement     = hullVertices.Elements[down];
            var backwardElement = hullVertices.Elements[backward];
            var forwardElement  = hullVertices.Elements[forward];
            Vector3.Add(ref rightElement, ref rightDirection, out rightElement);
            Vector3.Subtract(ref leftElement, ref rightDirection, out leftElement);
            Vector3.Add(ref upElement, ref upDirection, out upElement);
            Vector3.Subtract(ref downElement, ref upDirection, out downElement);
            Vector3.Add(ref backwardElement, ref backDirection, out backwardElement);
            Vector3.Subtract(ref forwardElement, ref backDirection, out forwardElement);

            //Rather than transforming each axis independently (and doing three times as many operations as required), just get the 6 required values directly.
            TransformLocalExtremePoints(ref rightElement, ref upElement, ref backwardElement, ref o, out boundingBox.Max);
            TransformLocalExtremePoints(ref leftElement, ref downElement, ref forwardElement, ref o, out boundingBox.Min);
        }
Esempio n. 13
0
        public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
        {
            var box0 = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Box;
            var box1 = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Box;

            var oldBox0 = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Box;
            var oldBox1 = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Box;

            var dirs0 = box0.Orientation;
            var dirs1 = box1.Orientation;

            var box0_Right    = dirs0.Right;
            var box0_Up       = dirs0.Up;
            var box0_Backward = dirs0.Backward;

            var box1_Right    = dirs1.Right;
            var box1_Up       = dirs1.Up;
            var box1_Backward = dirs1.Backward;

            if (Disjoint(out var testDepth, ref box0_Right, box0, box1, collTolerance))
            {
                return;
            }

            var depth   = testDepth;
            var N       = box0_Right;
            var minAxis = 0;

            if (Disjoint(out testDepth, ref box0_Up, box0, box1, collTolerance))
            {
                return;
            }

            if (testDepth < depth)
            {
                depth   = testDepth;
                N       = box0_Up;
                minAxis = 1;
            }

            if (Disjoint(out testDepth, ref box0_Backward, box0, box1, collTolerance))
            {
                return;
            }

            if (testDepth < depth)
            {
                depth   = testDepth;
                N       = box0_Backward;
                minAxis = 2;
            }

            if (Disjoint(out testDepth, ref box1_Right, box0, box1, collTolerance))
            {
                return;
            }

            if (testDepth < depth)
            {
                depth   = testDepth;
                N       = box1_Right;
                minAxis = 3;
            }

            if (Disjoint(out testDepth, ref box1_Up, box0, box1, collTolerance))
            {
                return;
            }

            if (testDepth < depth)
            {
                depth   = testDepth;
                N       = box1_Up;
                minAxis = 4;
            }

            if (Disjoint(out testDepth, ref box1_Backward, box0, box1, collTolerance))
            {
                return;
            }

            if (testDepth < depth)
            {
                depth   = testDepth;
                N       = box1_Backward;
                minAxis = 5;
            }

            Vector3.Cross(ref box0_Right, ref box1_Right, out var axis);
            if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
            {
                return;
            }

            testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
            if (testDepth < depth)
            {
                depth   = testDepth;
                N       = axis;
                minAxis = 6;
            }

            Vector3.Cross(ref box0_Right, ref box1_Up, out axis);
            if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
            {
                return;
            }

            testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
            if (testDepth < depth)
            {
                depth   = testDepth;
                N       = axis;
                minAxis = 7;
            }

            Vector3.Cross(ref box0_Right, ref box1_Backward, out axis);
            if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
            {
                return;
            }

            testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
            if (testDepth < depth)
            {
                depth   = testDepth;
                N       = axis;
                minAxis = 8;
            }

            Vector3.Cross(ref box0_Up, ref box1_Right, out axis);
            if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
            {
                return;
            }

            testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
            if (testDepth < depth)
            {
                depth   = testDepth;
                N       = axis;
                minAxis = 9;
            }

            Vector3.Cross(ref box0_Up, ref box1_Up, out axis);
            if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
            {
                return;
            }

            testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
            if (testDepth < depth)
            {
                depth   = testDepth;
                N       = axis;
                minAxis = 10;
            }

            Vector3.Cross(ref box0_Up, ref box1_Backward, out axis);
            if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
            {
                return;
            }

            testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
            if (testDepth < depth)
            {
                depth   = testDepth;
                N       = axis;
                minAxis = 11;
            }

            Vector3.Cross(ref box0_Backward, ref box1_Right, out axis);
            if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
            {
                return;
            }

            testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
            if (testDepth < depth)
            {
                depth   = testDepth;
                N       = axis;
                minAxis = 12;
            }

            Vector3.Cross(ref box0_Backward, ref box1_Up, out axis);
            if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
            {
                return;
            }

            testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
            if (testDepth < depth)
            {
                depth   = testDepth;
                N       = axis;
                minAxis = 13;
            }

            Vector3.Cross(ref box0_Backward, ref box1_Backward, out axis);
            if (Disjoint(out testDepth, ref axis, box0, box1, collTolerance))
            {
                return;
            }

            testDepth *= 1.0f / (float)System.Math.Sqrt(axis.X * axis.X + axis.Y * axis.Y + axis.Z * axis.Z);
            if (testDepth < depth)
            {
                depth   = testDepth;
                N       = axis;
                minAxis = 14;
            }

            var D = box1.GetCentre() - box0.GetCentre();

            N.Normalize();
            int i;

            /*seperatingAxes[0] = dirs0.Right;
             * seperatingAxes[1] = dirs0.Up;
             * seperatingAxes[2] = dirs0.Backward;
             * seperatingAxes[3] = dirs1.Right;
             * seperatingAxes[4] = dirs1.Up;
             * seperatingAxes[5] = dirs1.Backward;
             * Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[3], out seperatingAxes[6]);
             * Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[4], out seperatingAxes[7]);
             * Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[5], out seperatingAxes[8]);
             * Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[3], out seperatingAxes[9]);
             * Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[4], out seperatingAxes[10]);
             * Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[5], out seperatingAxes[11]);
             * Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[3], out seperatingAxes[12]);
             * Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[4], out seperatingAxes[13]);
             * Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[5], out seperatingAxes[14]);
             *
             *
             *
             *
             * int i;
             * for (i = 0; i < 15; ++i)
             * {
             *
             *  float l2 = seperatingAxes[i].LengthSquared();
             *
             *  if (l2 < JiggleMath.Epsilon) continue;
             *
             *  overlapDepth[i] = float.MaxValue;
             *
             *  if (Disjoint(out overlapDepth[i], ref seperatingAxes[i], box0, box1, collTolerance))
             *      return;
             * }
             *
             *
             * float minDepth = float.MaxValue;
             * int minAxis = -1;
             *
             * for (i = 0; i < 15; ++i)
             * {
             *
             *  float l2 = seperatingAxes[i].LengthSquared();
             *  if (l2 < JiggleMath.Epsilon) continue;
             *
             *
             *  float invl = 1.0f / (float)System.Math.Sqrt(l2);
             *  seperatingAxes[i] *= invl;
             *  overlapDepth[i] *= invl;
             *
             *
             *  if (overlapDepth[i] < minDepth)
             *  {
             *      minDepth = overlapDepth[i];
             *      minAxis = i;
             *  }
             * }
             *
             * if (minAxis == -1)
             *  return;
             *
             *
             *
             * Vector3 D = box1.GetCentre() - box0.GetCentre();
             * Vector3 N = seperatingAxes[minAxis];
             * float depth = overlapDepth[minAxis];*/

            if (Vector3.Dot(D, N) > 0.0f)
            {
                N *= -1.0f;
            }

            var minA = MathHelper.Min(box0.SideLengths.X, MathHelper.Min(box0.SideLengths.Y, box0.SideLengths.Z));
            var minB = MathHelper.Min(box1.SideLengths.X, MathHelper.Min(box1.SideLengths.Y, box1.SideLengths.Z));

            var combinationDist = 0.05f * MathHelper.Min(minA, minB);


            var contactPointsFromOld = true;

            contactPts.Clear();

            if (depth > -JiggleMath.Epsilon)
            {
                GetBoxBoxIntersectionPoints(contactPts, oldBox0, oldBox1, combinationDist, collTolerance);
            }

            var numPts = contactPts.Count;

            if (numPts == 0)
            {
                contactPointsFromOld = false;
                GetBoxBoxIntersectionPoints(contactPts, box0, box1, combinationDist, collTolerance);
            }

            numPts = contactPts.Count;

            var body0OldPos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero;
            var body1OldPos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero;
            var body0NewPos = info.Skin0.Owner?.Position ?? Vector3.Zero;
            var body1NewPos = info.Skin1.Owner?.Position ?? Vector3.Zero;

            Vector3.Subtract(ref body0NewPos, ref body0OldPos, out var bodyDelta);
            Vector3.Subtract(ref bodyDelta, ref body1NewPos, out bodyDelta);
            Vector3.Add(ref bodyDelta, ref body1OldPos, out bodyDelta);

            Vector3.Dot(ref bodyDelta, ref N, out var bodyDeltaLen);

            var oldDepth = depth + bodyDeltaLen;

            unsafe
            {
                var collPts = stackalloc SmallCollPointInfo[MaxLocalStackScpi];
                {
                    var numCollPts = 0;

                    Vector3 SATPoint;

                    switch (minAxis)
                    {
                    case 0:
                    case 1:
                    case 2:
                    {
                        GetSupportPoint(out SATPoint, box1, -N);
                        break;
                    }

                    case 3:
                    case 4:
                    case 5:
                    {
                        GetSupportPoint(out SATPoint, box0, N);
                        break;
                    }

                    default:
                        /*case 6:
                         * case 7:
                         * case 8:
                         * case 9:
                         * case 10:
                         * case 11:
                         * case 12:
                         * case 13:
                         * case 14:*/
                    {
                        {
                            i = minAxis - 6;
                            var ia = i / 3;
                            var ib = i - ia * 3;

                            GetSupportPoint(out var P0, box0, N);
                            GetSupportPoint(out var P1, box1, -N);


                            JiggleUnsafe.Get(ref box0.transform.Orientation, ia, out var box0Orient);
                            JiggleUnsafe.Get(ref box1.transform.Orientation, ib, out var box1Orient);

                            Vector3.Cross(ref N, ref box1Orient, out var planeNormal);

                            Vector3.Dot(ref planeNormal, ref P1, out var planeD);


                            Vector3.Dot(ref box0Orient, ref planeNormal, out var div);


                            if (System.Math.Abs(div) < JiggleMath.Epsilon)
                            {
                                return;
                            }

                            var t = (planeD - Vector3.Dot(P0, planeNormal)) / div;


                            P0 = Vector3.Add(Vector3.Multiply(box0Orient, t), P0);

                            Vector3.Multiply(ref N, 0.5f * depth, out SATPoint);
                            Vector3.Add(ref SATPoint, ref P0, out SATPoint);
                        }
                        break;
                    }

                        /*default:
                         *  throw new Exception("Impossible switch");*/
                    }


                    if (numPts > 0)
                    {
                        var minDist = float.MaxValue;
                        var maxDist = float.MinValue;
                        for (i = 0; i < numPts; ++i)
                        {
                            var dist = Distance.PointPointDistance(contactPts[i].Pos, SATPoint);
                            if (dist < minDist)
                            {
                                minDist = dist;
                            }
                            if (dist > maxDist)
                            {
                                maxDist = dist;
                            }
                        }

                        if (maxDist < minDist + JiggleMath.Epsilon)
                        {
                            maxDist = minDist + JiggleMath.Epsilon;
                        }


                        for (i = 0; i < numPts; ++i)
                        {
                            var minDepthScale = 0.0f;
                            var dist          = Distance.PointPointDistance(contactPts[i].Pos, SATPoint);

                            var depthDiv   = System.Math.Max(JiggleMath.Epsilon, maxDist - minDist);
                            var depthScale = (dist - minDist) / depthDiv;

                            depth = (1.0f - depthScale) * oldDepth + minDepthScale * depthScale * oldDepth;

                            if (contactPointsFromOld)
                            {
                                if (numCollPts < MaxLocalStackScpi)
                                {
                                    collPts[numCollPts].R0 = contactPts[i].Pos - body0OldPos;
                                    collPts[numCollPts].R1 = contactPts[i].Pos - body1OldPos;
                                    collPts[numCollPts++].InitialPenetration = depth;
                                }
                            }
                            else
                            {
                                if (numCollPts < MaxLocalStackScpi)
                                {
                                    collPts[numCollPts].R0 = contactPts[i].Pos - body0NewPos;
                                    collPts[numCollPts].R1 = contactPts[i].Pos - body1NewPos;
                                    collPts[numCollPts++].InitialPenetration = depth;
                                }
                            }
                        }
                    }
                    else
                    {
                        Vector3.Subtract(ref SATPoint, ref body0NewPos, out var cp0);

                        Vector3.Subtract(ref SATPoint, ref body1NewPos, out var cp1);

                        if (numCollPts < MaxLocalStackScpi)
                        {
                            collPts[numCollPts].R0 = cp0;
                            collPts[numCollPts].R1 = cp1;
                            collPts[numCollPts++].InitialPenetration = oldDepth;
                        }
                    }


                    collisionFunctor.CollisionNotify(ref info, ref N, collPts, numCollPts);
                }
            }
        }
Esempio n. 14
0
  public static void Main (String[] args) {

    spheres[0] = (new Sphere(
          new Vector3(0, -10002, 0),
          9999,
          new Vector3(1, 1, 1),
          false));

    spheres[1] = (new Sphere(
          new Vector3(-10012, 0, 0),
          9999,
          new Vector3(1, 0, 0),
          false));

    spheres[2] = (new Sphere(
          new Vector3(10012, 0, 0),
          9999,
          new Vector3(0, 1, 0),
          false));

    spheres[3] = (new Sphere(
          new Vector3(0, 0, -10012),
          9999,
          new Vector3(1, 1, 1),
          false));

    spheres[4] = (new Sphere(
          new Vector3(0, 10012, 0),
          9999,
          new Vector3(1, 1, 1),
          true));

    spheres[5] = (new Sphere(
          new Vector3(-5, 0, 2),
          2,
          new Vector3(1, 1, 0),
          false));

    spheres[6] = (new Sphere(
          new Vector3(0, 5, -1),
          4,
          new Vector3(1, 0, 0),
          false));

    spheres[7] = (new Sphere(
          new Vector3(8, 5, -1),
          2,
          new Vector3(0, 0, 1),
          false));

    var data = new Vector3[RayBench.HEIGHT][];
    var cam = new Camera();
    var vdu = cam.rt.Sub(cam.lt).Div(RayBench.WIDTH);
    var vdv = cam.lb.Sub(cam.lt).Div(RayBench.HEIGHT);

    var options = new ParallelOptions();
    options.MaxDegreeOfParallelism = 4;

    //for(int y = 0; y < RayBench.HEIGHT; ++y) {
    Parallel.For(0, RayBench.HEIGHT, options, y => {
      data[y] = new Vector3[RayBench.WIDTH];
      for(int x = 0; x < RayBench.WIDTH; ++x) {
        var color = new Vector3();
        var ray = new Ray();

        ray.origin = cam.eye;

        for(int i = 0; i < RayBench.SAMPLES; ++i) {
          ray.direction = cam.lt.Add(
              vdu.Mul((float)(x + rnd.NextDouble())).Add(
                vdv.Mul((float)(y + rnd.NextDouble()))));

          ray.direction = ray.direction.Sub(ray.origin);
          ray.direction = ray.direction.Unit();
          color = color.Add(RayBench.trace(ray, 0));
        }

        color = color.Div(RayBench.SAMPLES);

        data[y][x] = color;
      }
    });

    RayBench.WritePPM(data);
  }
Esempio n. 15
0
        /// <summary>
        /// Checks two shapes for collisions.
        /// </summary>
        /// <param name="support1">The SupportMappable implementation of the first shape to test.</param>
        /// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param>
        /// <param name="orientation1">The orientation of the first shape.</param>
        /// <param name="orientation2">The orientation of the second shape.</param>
        /// <param name="position1">The position of the first shape.</param>
        /// <param name="position2">The position of the second shape</param>
        /// <param name="point">The pointin world coordinates, where collision occur.</param>
        /// <param name="normal">The normal pointing from body2 to body1.</param>
        /// <param name="penetration">Estimated penetration depth of the collision.</param>
        /// <returns>Returns true if there is a collision, false otherwise.</returns>
        public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref Matrix3x3 orientation1,
                                  ref Matrix3x3 orientation2, ref Vector3 position1, ref Vector3 position2,
                                  out Vector3 point, out Vector3 normal, out float penetration)
        {
            // Used variables
            Vector3 temp1, temp2;
            Vector3 v01, v02, v0;
            Vector3 v11, v12, v1;
            Vector3 v21, v22, v2;
            Vector3 v31, v32, v3;
            Vector3 v41, v42, v4;
            Vector3 mn;

            // Initialization of the output
            point       = normal = Vector3.zero;
            penetration = 0.0f;

            //JVector right = JVector.Right;

            // Get the center of shape1 in world coordinates -> v01
            support1.SupportCenter(out v01);
            Vector3.Transform(ref v01, ref orientation1, out v01);
            Vector3.Add(ref position1, ref v01, out v01);

            // Get the center of shape2 in world coordinates -> v02
            support2.SupportCenter(out v02);
            Vector3.Transform(ref v02, ref orientation2, out v02);
            Vector3.Add(ref position2, ref v02, out v02);

            // v0 is the center of the minkowski difference
            Vector3.Subtract(ref v02, ref v01, out v0);

            // Avoid case where centers overlap -- any direction is fine in this case
            if (v0.IsNearlyZero())
            {
                v0 = new Vector3(0.00001f, 0, 0);
            }

            // v1 = support in direction of origin
            mn = v0;
            Vector3.Negate(ref v0, out normal);

            SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
            SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
            Vector3.Subtract(ref v12, ref v11, out v1);

            if (Vector3.Dot(ref v1, ref normal) <= 0.0f)
            {
                return(false);
            }

            // v2 = support perpendicular to v1,v0
            Vector3.Cross(ref v1, ref v0, out normal);

            if (normal.IsNearlyZero())
            {
                Vector3.Subtract(ref v1, ref v0, out normal);

                normal.Normalize();

                point = v11;
                Vector3.Add(ref point, ref v12, out point);
                Vector3.Multiply(ref point, 0.5f, out point);

                Vector3.Subtract(ref v12, ref v11, out temp1);
                penetration = Vector3.Dot(ref temp1, ref normal);

                //point = v11;
                //point2 = v12;
                return(true);
            }

            Vector3.Negate(ref normal, out mn);
            SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21);
            SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22);
            Vector3.Subtract(ref v22, ref v21, out v2);

            if (Vector3.Dot(ref v2, ref normal) <= 0.0f)
            {
                return(false);
            }

            // Determine whether origin is on + or - side of plane (v1,v0,v2)
            Vector3.Subtract(ref v1, ref v0, out temp1);
            Vector3.Subtract(ref v2, ref v0, out temp2);
            Vector3.Cross(ref temp1, ref temp2, out normal);

            float dist = Vector3.Dot(ref normal, ref v0);

            // If the origin is on the - side of the plane, reverse the direction of the plane
            if (dist > 0.0f)
            {
                Vector3.Swap(ref v1, ref v2);
                Vector3.Swap(ref v11, ref v21);
                Vector3.Swap(ref v12, ref v22);
                Vector3.Negate(ref normal, out normal);
            }


            int  phase2 = 0;
            int  phase1 = 0;
            bool hit    = false;

            // Phase One: Identify a portal
            while (true)
            {
                if (phase1 > MaximumIterations)
                {
                    return(false);
                }

                phase1++;

                // Obtain the support point in a direction perpendicular to the existing plane
                // Note: This point is guaranteed to lie off the plane
                Vector3.Negate(ref normal, out mn);
                SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
                SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
                Vector3.Subtract(ref v32, ref v31, out v3);

                if (Vector3.Dot(ref v3, ref normal) <= 0.0f)
                {
                    return(false);
                }

                // If origin is outside (v1,v0,v3), then eliminate v2 and loop
                Vector3.Cross(ref v1, ref v3, out temp1);
                if (Vector3.Dot(ref temp1, ref v0) < 0.0f)
                {
                    v2  = v3;
                    v21 = v31;
                    v22 = v32;
                    Vector3.Subtract(ref v1, ref v0, out temp1);
                    Vector3.Subtract(ref v3, ref v0, out temp2);
                    Vector3.Cross(ref temp1, ref temp2, out normal);
                    continue;
                }

                // If origin is outside (v3,v0,v2), then eliminate v1 and loop
                Vector3.Cross(ref v3, ref v2, out temp1);
                if (Vector3.Dot(ref temp1, ref v0) < 0.0f)
                {
                    v1  = v3;
                    v11 = v31;
                    v12 = v32;
                    Vector3.Subtract(ref v3, ref v0, out temp1);
                    Vector3.Subtract(ref v2, ref v0, out temp2);
                    Vector3.Cross(ref temp1, ref temp2, out normal);
                    continue;
                }

                // Phase Two: Refine the portal
                // We are now inside of a wedge...
                while (true)
                {
                    phase2++;

                    // Compute normal of the wedge face
                    Vector3.Subtract(ref v2, ref v1, out temp1);
                    Vector3.Subtract(ref v3, ref v1, out temp2);
                    Vector3.Cross(ref temp1, ref temp2, out normal);

                    // Can this happen???  Can it be handled more cleanly?
                    if (normal.IsNearlyZero())
                    {
                        return(true);
                    }

                    normal.Normalize();

                    // Compute distance from origin to wedge face
                    float d = Vector3.Dot(ref normal, ref v1);


                    // If the origin is inside the wedge, we have a hit
                    if (d >= 0 && !hit)
                    {
                        // HIT!!!
                        hit = true;
                    }

                    // Find the support point in the direction of the wedge face
                    Vector3.Negate(ref normal, out mn);
                    SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v41);
                    SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v42);
                    Vector3.Subtract(ref v42, ref v41, out v4);

                    Vector3.Subtract(ref v4, ref v3, out temp1);
                    float delta = Vector3.Dot(ref temp1, ref normal);
                    penetration = Vector3.Dot(ref v4, ref normal);

                    // If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
                    if (delta <= CollideEpsilon || penetration <= 0.0f || phase2 > MaximumIterations)
                    {
                        if (hit)
                        {
                            Vector3.Cross(ref v1, ref v2, out temp1);
                            float b0 = Vector3.Dot(ref temp1, ref v3);
                            Vector3.Cross(ref v3, ref v2, out temp1);
                            float b1 = Vector3.Dot(ref temp1, ref v0);
                            Vector3.Cross(ref v0, ref v1, out temp1);
                            float b2 = Vector3.Dot(ref temp1, ref v3);
                            Vector3.Cross(ref v2, ref v1, out temp1);
                            float b3 = Vector3.Dot(ref temp1, ref v0);

                            float sum = b0 + b1 + b2 + b3;

                            if (sum <= 0)
                            {
                                b0 = 0;
                                Vector3.Cross(ref v2, ref v3, out temp1);
                                b1 = Vector3.Dot(ref temp1, ref normal);
                                Vector3.Cross(ref v3, ref v1, out temp1);
                                b2 = Vector3.Dot(ref temp1, ref normal);
                                Vector3.Cross(ref v1, ref v2, out temp1);
                                b3 = Vector3.Dot(ref temp1, ref normal);

                                sum = b1 + b2 + b3;
                            }

                            float inv = 1.0f / sum;

                            Vector3.Multiply(ref v01, b0, out point);
                            Vector3.Multiply(ref v11, b1, out temp1);
                            Vector3.Add(ref point, ref temp1, out point);
                            Vector3.Multiply(ref v21, b2, out temp1);
                            Vector3.Add(ref point, ref temp1, out point);
                            Vector3.Multiply(ref v31, b3, out temp1);
                            Vector3.Add(ref point, ref temp1, out point);

                            Vector3.Multiply(ref v02, b0, out temp2);
                            Vector3.Add(ref temp2, ref point, out point);
                            Vector3.Multiply(ref v12, b1, out temp1);
                            Vector3.Add(ref point, ref temp1, out point);
                            Vector3.Multiply(ref v22, b2, out temp1);
                            Vector3.Add(ref point, ref temp1, out point);
                            Vector3.Multiply(ref v32, b3, out temp1);
                            Vector3.Add(ref point, ref temp1, out point);

                            Vector3.Multiply(ref point, inv * 0.5f, out point);
                        }

                        // Compute the barycentric coordinates of the origin
                        return(hit);
                    }

                    //// Compute the tetrahedron dividing face (v4,v0,v1)
                    //JVector.Cross(ref v4, ref v1, out temp1);
                    //float d1 = JVector.Dot(ref temp1, ref v0);


                    //// Compute the tetrahedron dividing face (v4,v0,v2)
                    //JVector.Cross(ref v4, ref v2, out temp1);
                    //float d2 = JVector.Dot(ref temp1, ref v0);


                    // Compute the tetrahedron dividing face (v4,v0,v3)
                    Vector3.Cross(ref v4, ref v0, out temp1);
                    float dot = Vector3.Dot(ref temp1, ref v1);

                    if (dot >= 0.0f)
                    {
                        dot = Vector3.Dot(ref temp1, ref v2);

                        if (dot >= 0.0f)
                        {
                            // Inside d1 & inside d2 ==> eliminate v1
                            v1  = v4;
                            v11 = v41;
                            v12 = v42;
                        }
                        else
                        {
                            // Inside d1 & outside d2 ==> eliminate v3
                            v3  = v4;
                            v31 = v41;
                            v32 = v42;
                        }
                    }
                    else
                    {
                        dot = Vector3.Dot(ref temp1, ref v3);

                        if (dot >= 0.0f)
                        {
                            // Outside d1 & inside d3 ==> eliminate v2
                            v2  = v4;
                            v21 = v41;
                            v22 = v42;
                        }
                        else
                        {
                            // Outside d1 & outside d3 ==> eliminate v1
                            v1  = v4;
                            v11 = v41;
                            v12 = v42;
                        }
                    }
                }
            }
        }
Esempio n. 16
0
        protected internal override void UpdateJacobiansAndVelocityBias()
        {
            //Transform local stuff into world space
            Vector3 worldRestrictedAxis1, worldRestrictedAxis2;

            Vector3.Transform(ref localRestrictedAxis1, ref ConnectionA.Orientation, out worldRestrictedAxis1);
            Vector3.Transform(ref localRestrictedAxis2, ref ConnectionA.Orientation, out worldRestrictedAxis2);

            Vector3 worldLineAnchor;

            Vector3.Transform(ref LocalLineAnchor, ref ConnectionA.Orientation, out worldLineAnchor);
            Vector3.Add(ref worldLineAnchor, ref ConnectionA.Position, out worldLineAnchor);
            Vector3 lineDirection;

            Vector3.Transform(ref localLineDirection, ref ConnectionA.Orientation, out lineDirection);

            Vector3 rB;

            Vector3.Transform(ref LocalAnchorB, ref ConnectionB.Orientation, out rB);
            Vector3 worldPoint;

            Vector3.Add(ref rB, ref ConnectionB.Position, out worldPoint);

            //Find the point on the line closest to the world point.
            Vector3 offset;

            Vector3.Subtract(ref worldPoint, ref worldLineAnchor, out offset);
            float distanceAlongAxis;

            Vector3.Dot(ref offset, ref lineDirection, out distanceAlongAxis);

            Vector3 worldNearPoint;

            Vector3.Multiply(ref lineDirection, distanceAlongAxis, out offset);
            Vector3.Add(ref worldLineAnchor, ref offset, out worldNearPoint);
            Vector3 rA;

            Vector3.Subtract(ref worldNearPoint, ref ConnectionA.Position, out rA);

            //Error
            Vector3 error3D;

            Vector3.Subtract(ref worldPoint, ref worldNearPoint, out error3D);

            Vector2 error;

            Vector3.Dot(ref error3D, ref worldRestrictedAxis1, out error.X);
            Vector3.Dot(ref error3D, ref worldRestrictedAxis2, out error.Y);

            velocityBias.X = errorCorrectionFactor * error.X;
            velocityBias.Y = errorCorrectionFactor * error.Y;


            //Set up the jacobians
            Vector3 angularA1, angularA2, angularB1, angularB2;

            Vector3.Cross(ref rA, ref worldRestrictedAxis1, out angularA1);
            Vector3.Cross(ref rA, ref worldRestrictedAxis2, out angularA2);
            Vector3.Cross(ref worldRestrictedAxis1, ref rB, out angularB1);
            Vector3.Cross(ref worldRestrictedAxis2, ref rB, out angularB2);

            //Put all the 1x3 jacobians into a 3x3 matrix representation.
            linearJacobianA = new Matrix3x3
            {
                M11 = worldRestrictedAxis1.X,
                M12 = worldRestrictedAxis1.Y,
                M13 = worldRestrictedAxis1.Z,
                M21 = worldRestrictedAxis2.X,
                M22 = worldRestrictedAxis2.Y,
                M23 = worldRestrictedAxis2.Z
            };
            Matrix3x3.Negate(ref linearJacobianA, out linearJacobianB);

            angularJacobianA = new Matrix3x3
            {
                M11 = angularA1.X,
                M12 = angularA1.Y,
                M13 = angularA1.Z,
                M21 = angularA2.X,
                M22 = angularA2.Y,
                M23 = angularA2.Z
            };
            angularJacobianB = new Matrix3x3
            {
                M11 = angularB1.X,
                M12 = angularB1.Y,
                M13 = angularB1.Z,
                M21 = angularB2.X,
                M22 = angularB2.Y,
                M23 = angularB2.Z
            };
        }
Esempio n. 17
0
        ///<summary>
        /// Performs the frame's configuration step.
        ///</summary>
        ///<param name="dt">Timestep duration.</param>
        public override void Update(float dt)
        {
            entityADynamic = entityA != null && entityA.isDynamic;
            entityBDynamic = entityB != null && entityB.isDynamic;

            contactCount = ContactManifoldConstraint.penetrationConstraints.Count;
            switch (contactCount)
            {
            case 1:
                manifoldCenter = ContactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position;
                break;

            case 2:
                Vector3.Add(ref ContactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position,
                            ref ContactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position,
                            out manifoldCenter);
                manifoldCenter.X *= .5f;
                manifoldCenter.Y *= .5f;
                manifoldCenter.Z *= .5f;
                break;

            case 3:
                Vector3.Add(ref ContactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position,
                            ref ContactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position,
                            out manifoldCenter);
                Vector3.Add(ref ContactManifoldConstraint.penetrationConstraints.Elements[2].contact.Position,
                            ref manifoldCenter,
                            out manifoldCenter);
                manifoldCenter.X *= .333333333f;
                manifoldCenter.Y *= .333333333f;
                manifoldCenter.Z *= .333333333f;
                break;

            case 4:
                //This isn't actually the center of the manifold.  Is it good enough?  Sure seems like it.
                Vector3.Add(ref ContactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position,
                            ref ContactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position,
                            out manifoldCenter);
                Vector3.Add(ref ContactManifoldConstraint.penetrationConstraints.Elements[2].contact.Position,
                            ref manifoldCenter,
                            out manifoldCenter);
                Vector3.Add(ref ContactManifoldConstraint.penetrationConstraints.Elements[3].contact.Position,
                            ref manifoldCenter,
                            out manifoldCenter);
                manifoldCenter.X *= .25f;
                manifoldCenter.Y *= .25f;
                manifoldCenter.Z *= .25f;
                break;

            default:
                manifoldCenter = Toolbox.NoVector;
                break;
            }

            //Compute the three dimensional relative velocity at the point.


            Vector3 velocityA, velocityB;

            if (entityA != null)
            {
                Vector3.Subtract(ref manifoldCenter, ref entityA.position, out ra);
                Vector3.Cross(ref entityA.angularVelocity, ref ra, out velocityA);
                Vector3.Add(ref velocityA, ref entityA.linearVelocity, out velocityA);
            }
            else
            {
                velocityA = new Vector3();
            }

            if (entityB != null)
            {
                Vector3.Subtract(ref manifoldCenter, ref entityB.position, out rb);
                Vector3.Cross(ref entityB.angularVelocity, ref rb, out velocityB);
                Vector3.Add(ref velocityB, ref entityB.linearVelocity, out velocityB);
            }
            else
            {
                velocityB = new Vector3();
            }

            Vector3.Subtract(ref velocityA, ref velocityB, out relativeVelocity);

            //Get rid of the normal velocity.
            Vector3 normal = ContactManifoldConstraint.penetrationConstraints.Elements[0].contact.Normal;
            float   normalVelocityScalar = normal.X * relativeVelocity.X + normal.Y * relativeVelocity.Y +
                                           normal.Z * relativeVelocity.Z;

            relativeVelocity.X -= normalVelocityScalar * normal.X;
            relativeVelocity.Y -= normalVelocityScalar * normal.Y;
            relativeVelocity.Z -= normalVelocityScalar * normal.Z;

            //Create the jacobian entry and decide the friction coefficient.
            float length = relativeVelocity.LengthSquared();

            if (length > Toolbox.Epsilon)
            {
                length = (float)Math.Sqrt(length);
                float inverseLength = 1 / length;
                linearA.M11 = relativeVelocity.X * inverseLength;
                linearA.M12 = relativeVelocity.Y * inverseLength;
                linearA.M13 = relativeVelocity.Z * inverseLength;


                friction = length > CollisionResponseSettings.StaticFrictionVelocityThreshold
                    ? ContactManifoldConstraint.materialInteraction.KineticFriction
                    : ContactManifoldConstraint.materialInteraction.StaticFriction;
            }
            else
            {
                friction = ContactManifoldConstraint.materialInteraction.StaticFriction;

                //If there was no velocity, try using the previous frame's jacobian... if it exists.
                //Reusing an old one is okay since jacobians are cleared when a contact is initialized.
                if (!(linearA.M11 != 0 || linearA.M12 != 0 || linearA.M13 != 0))
                {
                    //Otherwise, just redo it all.
                    //Create arbitrary axes.
                    Vector3 axis1;
                    Vector3.Cross(ref normal, ref Toolbox.RightVector, out axis1);
                    length = axis1.LengthSquared();
                    if (length > Toolbox.Epsilon)
                    {
                        length = (float)Math.Sqrt(length);
                        float inverseLength = 1 / length;
                        linearA.M11 = axis1.X * inverseLength;
                        linearA.M12 = axis1.Y * inverseLength;
                        linearA.M13 = axis1.Z * inverseLength;
                    }
                    else
                    {
                        Vector3.Cross(ref normal, ref Toolbox.UpVector, out axis1);
                        axis1.Normalize();
                        linearA.M11 = axis1.X;
                        linearA.M12 = axis1.Y;
                        linearA.M13 = axis1.Z;
                    }
                }
            }

            //Second axis is first axis x normal
            linearA.M21 = linearA.M12 * normal.Z - linearA.M13 * normal.Y;
            linearA.M22 = linearA.M13 * normal.X - linearA.M11 * normal.Z;
            linearA.M23 = linearA.M11 * normal.Y - linearA.M12 * normal.X;


            //Compute angular jacobians
            if (entityA != null)
            {
                //angularA 1 =  ra x linear axis 1
                angularA.M11 = ra.Y * linearA.M13 - ra.Z * linearA.M12;
                angularA.M12 = ra.Z * linearA.M11 - ra.X * linearA.M13;
                angularA.M13 = ra.X * linearA.M12 - ra.Y * linearA.M11;

                //angularA 2 =  ra x linear axis 2
                angularA.M21 = ra.Y * linearA.M23 - ra.Z * linearA.M22;
                angularA.M22 = ra.Z * linearA.M21 - ra.X * linearA.M23;
                angularA.M23 = ra.X * linearA.M22 - ra.Y * linearA.M21;
            }

            //angularB 1 =  linear axis 1 x rb
            if (entityB != null)
            {
                angularB.M11 = linearA.M12 * rb.Z - linearA.M13 * rb.Y;
                angularB.M12 = linearA.M13 * rb.X - linearA.M11 * rb.Z;
                angularB.M13 = linearA.M11 * rb.Y - linearA.M12 * rb.X;

                //angularB 2 =  linear axis 2 x rb
                angularB.M21 = linearA.M22 * rb.Z - linearA.M23 * rb.Y;
                angularB.M22 = linearA.M23 * rb.X - linearA.M21 * rb.Z;
                angularB.M23 = linearA.M21 * rb.Y - linearA.M22 * rb.X;
            }

            //Compute inverse effective mass matrix
            Matrix2x2 entryA, entryB;

            //these are the transformed coordinates
            Matrix2x3 transform;
            Matrix3x2 transpose;

            if (entityADynamic)
            {
                Matrix2x3.Multiply(ref angularA, ref entityA.inertiaTensorInverse, out transform);
                Matrix2x3.Transpose(ref angularA, out transpose);
                Matrix2x2.Multiply(ref transform, ref transpose, out entryA);
                entryA.M11 += entityA.inverseMass;
                entryA.M22 += entityA.inverseMass;
            }
            else
            {
                entryA = new Matrix2x2();
            }

            if (entityBDynamic)
            {
                Matrix2x3.Multiply(ref angularB, ref entityB.inertiaTensorInverse, out transform);
                Matrix2x3.Transpose(ref angularB, out transpose);
                Matrix2x2.Multiply(ref transform, ref transpose, out entryB);
                entryB.M11 += entityB.inverseMass;
                entryB.M22 += entityB.inverseMass;
            }
            else
            {
                entryB = new Matrix2x2();
            }

            velocityToImpulse.M11 = -entryA.M11 - entryB.M11;
            velocityToImpulse.M12 = -entryA.M12 - entryB.M12;
            velocityToImpulse.M21 = -entryA.M21 - entryB.M21;
            velocityToImpulse.M22 = -entryA.M22 - entryB.M22;
            Matrix2x2.Invert(ref velocityToImpulse, out velocityToImpulse);
        }
Esempio n. 18
0
        private void UpdateAbs(bool updateTempVel = false)
        {
            this.CheckValidTransform();

            if (this.parentTransform == null)
            {
                this.angleAbs = this.angle;
                this.posAbs   = this.pos;
                this.scaleAbs = this.scale;
                if (updateTempVel)
                {
                    this.tempVelAbs      = this.tempVel;
                    this.tempAngleVelAbs = this.tempAngleVel;
                }
            }
            else
            {
                if (this.deriveAngle)
                {
                    this.angleAbs = MathF.NormalizeAngle(this.angle + this.parentTransform.angleAbs);
                    if (updateTempVel)
                    {
                        this.tempAngleVelAbs = this.tempAngleVel + this.parentTransform.tempAngleVelAbs;
                    }
                }
                else
                {
                    this.angleAbs = this.angle;
                    if (updateTempVel)
                    {
                        this.tempAngleVelAbs = this.tempAngleVel;
                    }
                }

                this.scaleAbs = this.scale * this.parentTransform.scaleAbs;

                Vector2 parentAngleAbsDotX;
                Vector2 parentAngleAbsDotY;
                MathF.GetTransformDotVec(this.parentTransform.angleAbs, out parentAngleAbsDotX, out parentAngleAbsDotY);

                Vector3.Multiply(ref this.pos, this.parentTransform.scaleAbs, out this.posAbs);
                MathF.TransformDotVec(ref this.posAbs, ref parentAngleAbsDotX, ref parentAngleAbsDotY);
                Vector3.Add(ref this.posAbs, ref this.parentTransform.posAbs, out this.posAbs);

                if (updateTempVel)
                {
                    Vector2 parentTurnVelAdjust = this.pos.Xy.PerpendicularRight;
                    Vector2.Multiply(ref parentTurnVelAdjust, this.parentTransform.tempAngleVelAbs, out parentTurnVelAdjust);
                    MathF.TransformDotVec(ref parentTurnVelAdjust, ref parentAngleAbsDotX, ref parentAngleAbsDotY);

                    Vector3.Multiply(ref this.tempVel, this.parentTransform.scaleAbs, out this.tempVelAbs);
                    MathF.TransformDotVec(ref this.tempVelAbs, ref parentAngleAbsDotX, ref parentAngleAbsDotY);
                    Vector3.Add(ref this.tempVelAbs, ref this.parentTransform.tempVelAbs, out this.tempVelAbs);

                    this.tempVelAbs.X += parentTurnVelAdjust.X;
                    this.tempVelAbs.Y += parentTurnVelAdjust.Y;
                }
            }

            // Update absolute children coordinates
            this.UpdateAbsChild(updateTempVel);

            this.CheckValidTransform();
        }
Esempio n. 19
0
        public void Align(Vector3 v1, Vector3 v2)
        {
            // If V1 and V2 are not parallel, the axis of rotation is the unit-length
            // vector U = Cross(V1,V2)/Length(Cross(V1,V2)).  The angle of rotation,
            // A, is the angle between V1 and V2.  The quaternion for the rotation is
            // q = cos(A/2) + sin(A/2)*(ux*i+uy*j+uz*k) where U = (ux,uy,uz).
            //
            // (1) Rather than extract A = acos(Dot(V1,V2)), multiply by 1/2, then
            //	 compute sin(A/2) and cos(A/2), we reduce the computational costs by
            //	 computing the bisector B = (V1+V2)/Length(V1+V2), so cos(A/2) =
            //	 Dot(V1,B).
            //
            // (2) The rotation axis is U = Cross(V1,B)/Length(Cross(V1,B)), but
            //	 Length(Cross(V1,B)) = Length(V1)*Length(B)*sin(A/2) = sin(A/2), in
            //	 which case sin(A/2)*(ux*i+uy*j+uz*k) = (cx*i+cy*j+cz*k) where
            //	 C = Cross(V1,B).
            //
            // If V1 = V2, then B = V1, cos(A/2) = 1, and U = (0,0,0).  If V1 = -V2,
            // then B = 0.  This can happen even if V1 is approximately -V2 using
            // floating point arithmetic, since Vector3::Normalize checks for
            // closeness to zero and returns the zero vector accordingly.  The test
            // for exactly zero is usually not recommend for floating point
            // arithmetic, but the implementation of Vector3::Normalize guarantees
            // the comparison is robust.  In this case, the A = pi and any axis
            // perpendicular to V1 may be used as the rotation axis.

            var bisector = v1.Add(v2);
            bisector.Normalise();

            var cosHalfAngle = v1.Dot(bisector);
            Vector3 cross;

            tuple[0] = cosHalfAngle;

            if (cosHalfAngle != 0)
            {
                cross = v1.Cross(bisector);
                tuple[1] = cross.tuple[0];
                tuple[2] = cross.tuple[1];
                tuple[3] = cross.tuple[2];
            }
            else
            {
                double invLength;
                if (System.Math.Abs(v1.tuple[0]) >= System.Math.Abs(v1.tuple[1]))
                {
                    // V1.x or V1.z is the largest magnitude component
                    invLength = 1 / System.Math.Sqrt(v1.tuple[0] * v1.tuple[0]
                                            + v1.tuple[2] * v1.tuple[2]);
                    tuple[1] = - v1.tuple[2] * invLength;
                    tuple[2] = 0;
                    tuple[3] = + v1.tuple[0] * invLength;
                }
                else
                {
                    // V1.y or V1.z is the largest magnitude component
                    invLength = 1 / System.Math.Sqrt(v1.tuple[1] * v1.tuple[1]
                                            + v1.tuple[2] * v1.tuple[2]);
                    tuple[1] = 0;
                    tuple[2] = + v1.tuple[2] * invLength;
                    tuple[3] = - v1.tuple[1] * invLength;
                }
            }
        }
Esempio n. 20
0
        /// <summary>
        /// Splits a single compound collidable into two separate compound collidables and computes information needed by the simulation.
        /// </summary>
        /// <param name="splitPredicate">Delegate which determines if a child in the original compound should be moved to the new compound.</param>
        /// <param name="a">Original compound to be split.  Children in this compound will be removed and added to the other compound.</param>
        /// <param name="b">Compound to receive children removed from the original compound.</param>
        /// <param name="distributionInfoA">Volume, volume distribution, and center information about the new form of the original compound collidable.</param>
        /// <param name="distributionInfoB">Volume, volume distribution, and center information about the new compound collidable.</param>
        /// <param name="weightA">Total weight associated with the new form of the original compound collidable.</param>
        /// <param name="weightB">Total weight associated with the new compound collidable.</param>
        /// <returns>Whether or not the predicate returned true for any element in the original compound and split the compound.</returns>
        public static bool SplitCompound(Func <CompoundChild, bool> splitPredicate,
                                         CompoundCollidable a, CompoundCollidable b,
                                         out ShapeDistributionInformation distributionInfoA, out ShapeDistributionInformation distributionInfoB,
                                         out float weightA, out float weightB)
        {
            bool splitOccurred = false;

            for (int i = a.children.Count - 1; i >= 0; i--)
            {
                //The shape doesn't change during this process.  The entity could, though.
                //All of the other collidable information, like the Tag, CollisionRules, Events, etc. all stay the same.
                var child = a.children.Elements[i];
                if (splitPredicate(child))
                {
                    splitOccurred = true;

                    a.children.FastRemoveAt(i);
                    b.children.Add(child);
                    //The child event handler must be unhooked from the old compound and given to the new one.
                    child.CollisionInformation.events.Parent = b.Events;
                }
            }

            if (!splitOccurred)
            {
                //No split occurred, so we cannot proceed.
                distributionInfoA = new ShapeDistributionInformation();
                distributionInfoB = new ShapeDistributionInformation();
                weightA           = 0;
                weightB           = 0;
                return(false);
            }

            //Compute the contributions from the original shape to the new form of the original collidable.
            distributionInfoA = new ShapeDistributionInformation();
            weightA           = 0;
            distributionInfoB = new ShapeDistributionInformation();
            weightB           = 0;
            for (int i = a.children.Count - 1; i >= 0; i--)
            {
                var     child = a.children.Elements[i];
                var     entry = child.Entry;
                Vector3 weightedCenter;
                Vector3.Multiply(ref entry.LocalTransform.Position, entry.Weight, out weightedCenter);
                Vector3.Add(ref weightedCenter, ref distributionInfoA.Center, out distributionInfoA.Center);
                distributionInfoA.Volume += entry.Shape.Volume;
                weightA += entry.Weight;
            }
            for (int i = b.children.Count - 1; i >= 0; i--)
            {
                var     child = b.children.Elements[i];
                var     entry = child.Entry;
                Vector3 weightedCenter;
                Vector3.Multiply(ref entry.LocalTransform.Position, entry.Weight, out weightedCenter);
                Vector3.Add(ref weightedCenter, ref distributionInfoB.Center, out distributionInfoB.Center);
                distributionInfoB.Volume += entry.Shape.Volume;
                weightB += entry.Weight;
            }

            //Average the center out.
            if (weightA > 0)
            {
                Vector3.Divide(ref distributionInfoA.Center, weightA, out distributionInfoA.Center);
            }

            if (weightB > 0)
            {
                Vector3.Divide(ref distributionInfoB.Center, weightB, out distributionInfoB.Center);
            }

            //Note that the 'entry' is from the Shape, and so the translations are local to the shape's center.
            //That is not technically the center of the new collidable- distributionInfoA.Center is.
            //Offset the child collidables by -distributionInfoA.Center using their local offset.
            Vector3 offsetA;

            Vector3.Negate(ref distributionInfoA.Center, out offsetA);
            Vector3 offsetB;

            Vector3.Negate(ref distributionInfoB.Center, out offsetB);

            //Compute the unscaled inertia tensor.
            for (int i = a.children.Count - 1; i >= 0; i--)
            {
                var        child = a.children.Elements[i];
                var        entry = child.Entry;
                Vector3    transformedOffset;
                Quaternion conjugate;
                Quaternion.Conjugate(ref entry.LocalTransform.Orientation, out conjugate);
                Quaternion.Transform(ref offsetA, ref conjugate, out transformedOffset);
                child.CollisionInformation.localPosition = transformedOffset;
                Matrix3x3 contribution;
                CompoundShape.TransformContribution(ref entry.LocalTransform, ref distributionInfoA.Center, ref entry.Shape.volumeDistribution, entry.Weight, out contribution);
                Matrix3x3.Add(ref contribution, ref distributionInfoA.VolumeDistribution, out distributionInfoA.VolumeDistribution);
            }
            for (int i = b.children.Count - 1; i >= 0; i--)
            {
                var        child = b.children.Elements[i];
                var        entry = child.Entry;
                Vector3    transformedOffset;
                Quaternion conjugate;
                Quaternion.Conjugate(ref entry.LocalTransform.Orientation, out conjugate);
                Quaternion.Transform(ref offsetB, ref conjugate, out transformedOffset);
                child.CollisionInformation.localPosition = transformedOffset;
                Matrix3x3 contribution;
                CompoundShape.TransformContribution(ref entry.LocalTransform, ref distributionInfoB.Center, ref entry.Shape.volumeDistribution, entry.Weight, out contribution);
                Matrix3x3.Add(ref contribution, ref distributionInfoB.VolumeDistribution, out distributionInfoB.VolumeDistribution);
            }

            //Normalize the volume distribution.
            Matrix3x3.Multiply(ref distributionInfoA.VolumeDistribution, 1 / weightA, out distributionInfoA.VolumeDistribution);
            Matrix3x3.Multiply(ref distributionInfoB.VolumeDistribution, 1 / weightB, out distributionInfoB.VolumeDistribution);

            //Update the hierarchies of the compounds.
            //TODO: Create a new method that does this quickly without garbage.  Requires a new Reconstruct method which takes a pool which stores the appropriate node types.
            a.hierarchy.Tree.Reconstruct(a.children);
            b.hierarchy.Tree.Reconstruct(b.children);

            return(true);
        }
Esempio n. 21
0
        ///<summary>
        /// Gets the point on the triangle closest to the origin.
        ///</summary>
        ///<param name="point">Point closest to origin.</param>
        public void GetPointOnTriangleClosestToOrigin(out Vector3 point)
        {
            Vector3 ab, ac;

            Vector3.Subtract(ref B, ref A, out ab);
            Vector3.Subtract(ref C, ref A, out ac);
            //The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector,
            //just use -A.
            //Same for B->P, C->P...

            //Check to see if it's outside A.
            //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside A.
            float AdotAB, AdotAC;

            Vector3.Dot(ref ab, ref A, out AdotAB);
            Vector3.Dot(ref ac, ref A, out AdotAC);
            AdotAB = -AdotAB;
            AdotAC = -AdotAC;
            if (AdotAC <= 0f && AdotAB <= 0)
            {
                //It is A!
                State = SimplexState.Point;
                U     = 1;
                point = A;
                return;
            }

            //Check to see if it's outside B.
            //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside B.
            float BdotAB, BdotAC;

            Vector3.Dot(ref ab, ref B, out BdotAB);
            Vector3.Dot(ref ac, ref B, out BdotAC);
            BdotAB = -BdotAB;
            BdotAC = -BdotAC;
            if (BdotAB >= 0f && BdotAC <= BdotAB)
            {
                //It is B!
                State      = SimplexState.Point;
                A          = B;
                U          = 1;
                SimplexA.A = SimplexA.B;
                SimplexB.A = SimplexB.B;
                point      = B;
                return;
            }

            //Check to see if it's outside AB.
            float vc = AdotAB * BdotAC - BdotAB * AdotAC;

            if (vc <= 0 && AdotAB > 0 && BdotAB < 0)//Note > and < instead of => <=; avoids possibly division by zero
            {
                State = SimplexState.Segment;
                V     = AdotAB / (AdotAB - BdotAB);
                U     = 1 - V;

                Vector3.Multiply(ref ab, V, out point);
                Vector3.Add(ref point, ref A, out point);
                return;
            }

            //Check to see if it's outside C.
            //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C.
            float CdotAB, CdotAC;

            Vector3.Dot(ref ab, ref C, out CdotAB);
            Vector3.Dot(ref ac, ref C, out CdotAC);
            CdotAB = -CdotAB;
            CdotAC = -CdotAC;
            if (CdotAC >= 0f && CdotAB <= CdotAC)
            {
                //It is C!
                State      = SimplexState.Point;
                A          = C;
                SimplexA.A = SimplexA.C;
                SimplexB.A = SimplexB.C;
                U          = 1;
                point      = A;
                return;
            }

            //Check if it's outside AC.
            //float AdotAB, AdotAC;
            //Vector3.Dot(ref ab, ref A, out AdotAB);
            //Vector3.Dot(ref ac, ref A, out AdotAC);
            //AdotAB = -AdotAB;
            //AdotAC = -AdotAC;
            float vb = CdotAB * AdotAC - AdotAB * CdotAC;

            if (vb <= 0f && AdotAC > 0f && CdotAC < 0f)//Note > instead of >= and < instead of <=; prevents bad denominator
            {
                //Get rid of B.  Compress C into B.
                State      = SimplexState.Segment;
                B          = C;
                SimplexA.B = SimplexA.C;
                SimplexB.B = SimplexB.C;
                V          = AdotAC / (AdotAC - CdotAC);
                U          = 1 - V;
                Vector3.Multiply(ref ac, V, out point);
                Vector3.Add(ref point, ref A, out point);
                return;
            }

            //Check if it's outside BC.
            //float BdotAB, BdotAC;
            //Vector3.Dot(ref ab, ref B, out BdotAB);
            //Vector3.Dot(ref ac, ref B, out BdotAC);
            //BdotAB = -BdotAB;
            //BdotAC = -BdotAC;
            float va = BdotAB * CdotAC - CdotAB * BdotAC;
            float d3d4;
            float d6d5;

            if (va <= 0f && (d3d4 = BdotAC - BdotAB) > 0f && (d6d5 = CdotAB - CdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator
            {
                //Throw away A.  C->A.
                //TODO: Does B->A, C->B work better?
                State      = SimplexState.Segment;
                A          = C;
                SimplexA.A = SimplexA.C;
                SimplexB.A = SimplexB.C;
                U          = d3d4 / (d3d4 + d6d5);
                V          = 1 - U;

                Vector3 bc;
                Vector3.Subtract(ref C, ref B, out bc);
                Vector3.Multiply(ref bc, U, out point);
                Vector3.Add(ref point, ref B, out point);
                return;
            }


            //On the face of the triangle.
            float denom = 1f / (va + vb + vc);

            V = vb * denom;
            W = vc * denom;
            U = 1 - V - W;
            Vector3.Multiply(ref ab, V, out point);
            Vector3 acw;

            Vector3.Multiply(ref ac, W, out acw);
            Vector3.Add(ref A, ref point, out point);
            Vector3.Add(ref point, ref acw, out point);
        }
Esempio n. 22
0
        /// <summary>
        /// Removes a child from a compound collidable.
        /// </summary>
        /// <param name="compound">Compound collidable to remove a child from.</param>
        /// <param name="removalPredicate">Callback which analyzes a child and determines if it should be removed from the compound.</param>
        /// <param name="childContributions">Distribution contributions from all shapes in the compound shape.  This can include shapes which are not represented in the compound.</param>
        /// <param name="distributionInfo">Distribution information of the new compound.</param>
        /// <param name="weight">Total weight of the new compound.</param>
        /// <param name="removedWeight">Weight removed from the compound.</param>
        /// <param name="removedCenter">Center of the chunk removed from the compound.</param>
        /// <returns>Whether or not any removal took place.</returns>
        public static bool RemoveChildFromCompound(CompoundCollidable compound, Func <CompoundChild, bool> removalPredicate, IList <ShapeDistributionInformation> childContributions,
                                                   out ShapeDistributionInformation distributionInfo, out float weight, out float removedWeight, out Vector3 removedCenter)
        {
            bool removalOccurred = false;

            removedWeight = 0;
            removedCenter = new Vector3();
            for (int i = compound.children.Count - 1; i >= 0; i--)
            {
                //The shape doesn't change during this process.  The entity could, though.
                //All of the other collidable information, like the Tag, CollisionRules, Events, etc. all stay the same.
                var child = compound.children.Elements[i];
                if (removalPredicate(child))
                {
                    removalOccurred = true;
                    var entry = child.Entry;
                    removedWeight += entry.Weight;
                    Vector3 toAdd;
                    Vector3.Multiply(ref entry.LocalTransform.Position, entry.Weight, out toAdd);
                    Vector3.Add(ref removedCenter, ref toAdd, out removedCenter);
                    //The child event handler must be unhooked from the compound.
                    child.CollisionInformation.events.Parent = null;
                    compound.children.FastRemoveAt(i);
                }
            }

            if (!removalOccurred)
            {
                //No removal occurred, so we cannot proceed.
                distributionInfo = new ShapeDistributionInformation();
                weight           = 0;
                return(false);
            }
            if (removedWeight > 0)
            {
                Vector3.Divide(ref removedCenter, removedWeight, out removedCenter);
            }

            //Compute the contributions from the original shape to the new form of the original collidable.
            distributionInfo = new ShapeDistributionInformation();
            weight           = 0;
            for (int i = compound.children.Count - 1; i >= 0; i--)
            {
                var child        = compound.children.Elements[i];
                var entry        = child.Entry;
                var contribution = childContributions[child.shapeIndex];
                Vector3.Add(ref contribution.Center, ref entry.LocalTransform.Position, out contribution.Center);
                Vector3.Multiply(ref contribution.Center, child.Entry.Weight, out contribution.Center);
                Vector3.Add(ref contribution.Center, ref distributionInfo.Center, out distributionInfo.Center);
                distributionInfo.Volume += contribution.Volume;
                weight += entry.Weight;
            }
            //Average the center out.
            Vector3.Divide(ref distributionInfo.Center, weight, out distributionInfo.Center);

            //Note that the 'entry' is from the Shape, and so the translations are local to the shape's center.
            //That is not technically the center of the new collidable- distributionInfo.Center is.
            //Offset the child collidables by -distributionInfo.Center using their local offset.
            Vector3 offset;

            Vector3.Negate(ref distributionInfo.Center, out offset);

            //Compute the unscaled inertia tensor.
            for (int i = compound.children.Count - 1; i >= 0; i--)
            {
                var        child = compound.children.Elements[i];
                var        entry = child.Entry;
                Vector3    transformedOffset;
                Quaternion conjugate;
                Quaternion.Conjugate(ref entry.LocalTransform.Orientation, out conjugate);
                Quaternion.Transform(ref offset, ref conjugate, out transformedOffset);
                child.CollisionInformation.localPosition = transformedOffset;
                var contribution = childContributions[child.shapeIndex];
                CompoundShape.TransformContribution(ref entry.LocalTransform, ref distributionInfo.Center, ref contribution.VolumeDistribution, entry.Weight, out contribution.VolumeDistribution);
                //Vector3.Add(ref entry.LocalTransform.Position, ref offsetA, out entry.LocalTransform.Position);
                Matrix3x3.Add(ref contribution.VolumeDistribution, ref distributionInfo.VolumeDistribution, out distributionInfo.VolumeDistribution);
            }

            //Normalize the volume distribution.
            Matrix3x3.Multiply(ref distributionInfo.VolumeDistribution, 1 / weight, out distributionInfo.VolumeDistribution);

            //Update the hierarchies of the compounds.
            //TODO: Create a new method that does this quickly without garbage.  Requires a new Reconstruct method which takes a pool which stores the appropriate node types.
            compound.hierarchy.Tree.Reconstruct(compound.children);

            return(true);
        }
Esempio n. 23
0
        /// <summary>
        /// Calculates necessary information for velocity solving.
        /// </summary>
        /// <param name="dt">Time in seconds since the last update.</param>
        public override void Update(float dt)
        {
            //Transform the anchors and offsets into world space.
            Matrix3X3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out offsetA);
            Matrix3X3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out offsetB);
            Vector3.Add(ref connectionA.position, ref offsetA, out anchorA);
            Vector3.Add(ref connectionB.position, ref offsetB, out anchorB);

            //Compute the distance.
            Vector3 separation;

            Vector3.Subtract(ref anchorB, ref anchorA, out separation);
            float currentDistance = separation.Length();

            //Compute jacobians
            if (currentDistance > Toolbox.Epsilon)
            {
                jLinearB.X = separation.X / currentDistance;
                jLinearB.Y = separation.Y / currentDistance;
                jLinearB.Z = separation.Z / currentDistance;
            }
            else
            {
                jLinearB = Toolbox.ZeroVector;
            }

            jLinearA.X = -jLinearB.X;
            jLinearA.Y = -jLinearB.Y;
            jLinearA.Z = -jLinearB.Z;

            Vector3.Cross(ref offsetA, ref jLinearB, out jAngularA);
            //Still need to negate angular A.  It's done after the effective mass matrix.
            Vector3.Cross(ref offsetB, ref jLinearB, out jAngularB);


            //Compute effective mass matrix
            if (connectionA.isDynamic && connectionB.isDynamic)
            {
                Vector3 aAngular;
                Matrix3X3.Transform(ref jAngularA, ref connectionA.localInertiaTensorInverse, out aAngular);
                Vector3.Cross(ref aAngular, ref offsetA, out aAngular);
                Vector3 bAngular;
                Matrix3X3.Transform(ref jAngularB, ref connectionB.localInertiaTensorInverse, out bAngular);
                Vector3.Cross(ref bAngular, ref offsetB, out bAngular);
                Vector3.Add(ref aAngular, ref bAngular, out aAngular);
                Vector3.Dot(ref aAngular, ref jLinearB, out velocityToImpulse);
                velocityToImpulse += connectionA.inverseMass + connectionB.inverseMass;
            }
            else if (connectionA.isDynamic)
            {
                Vector3 aAngular;
                Matrix3X3.Transform(ref jAngularA, ref connectionA.localInertiaTensorInverse, out aAngular);
                Vector3.Cross(ref aAngular, ref offsetA, out aAngular);
                Vector3.Dot(ref aAngular, ref jLinearB, out velocityToImpulse);
                velocityToImpulse += connectionA.inverseMass;
            }
            else if (connectionB.isDynamic)
            {
                Vector3 bAngular;
                Matrix3X3.Transform(ref jAngularB, ref connectionB.localInertiaTensorInverse, out bAngular);
                Vector3.Cross(ref bAngular, ref offsetB, out bAngular);
                Vector3.Dot(ref bAngular, ref jLinearB, out velocityToImpulse);
                velocityToImpulse += connectionB.inverseMass;
            }
            else
            {
                //No point in trying to solve with two kinematics.
                isActiveInSolver   = false;
                accumulatedImpulse = 0;
                return;
            }

            float errorReduction;

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

            velocityToImpulse = 1 / (softness + velocityToImpulse);
            //Finish computing jacobian; it's down here as an optimization (since it didn't need to be negated in mass matrix)
            jAngularA.X = -jAngularA.X;
            jAngularA.Y = -jAngularA.Y;
            jAngularA.Z = -jAngularA.Z;

            //Compute bias velocity
            error        = distance - currentDistance;
            biasVelocity = MathHelper.Clamp(error * errorReduction, -maxCorrectiveVelocity, maxCorrectiveVelocity);
        }
Esempio n. 24
0
        public override bool Apply(float dt)
        {
            Satisfied = true;

            if (body == null)
            {
                return(false);
            }

            float frac = 0.5f;

            if (frame == ReferenceFrame.Body) // transfrom velocity to the body frame
            {
                if (doVel)
                {
                    #region REFERENCE: Vector3 velBodyFrame = Vector3.Transform(vel, body.Orientation);
                    Vector3 velBodyFrame;
                    Vector3.Transform(ref vel, ref body.transform.Orientation, out velBodyFrame);
                    #endregion

                    #region REFERENCE: body.Velocity = (frac * velBodyFrame + (1.0f - frac) * body.Velocity);
                    Vector3 v1;
                    Vector3.Multiply(ref body.transformRate.Velocity, 1.0f - frac, out v1);
                    Vector3.Multiply(ref velBodyFrame, frac, out body.transformRate.Velocity);
                    Vector3.Add(ref body.transformRate.Velocity, ref v1, out body.transformRate.Velocity);
                    #endregion
                }

                if (doAngVel)
                {
                    #region REFERENCE: Vector3 angVelBodyFrame = Vector3.Transform(angVel, body.Orientation);
                    Vector3 angVelBodyFrame;
                    Vector3.Transform(ref angVel, ref body.transform.Orientation, out angVelBodyFrame);
                    #endregion

                    #region REFERENCE: body.AngVel = (frac * angVelBodyFrame + (1.0f - frac) * body.AngVel);
                    Vector3 v1;
                    Vector3.Multiply(ref body.transformRate.AngularVelocity, 1.0f - frac, out v1);
                    Vector3.Multiply(ref angVelBodyFrame, frac, out body.transformRate.AngularVelocity);
                    Vector3.Add(ref body.transformRate.AngularVelocity, ref v1, out body.transformRate.AngularVelocity);
                    #endregion
                }
            }
            else // leave velocity in the world frame
            {
                if (doVel)
                {
                    #region REFERENCE: body.Velocity = (frac * vel + (1.0f - frac) * body.Velocity);
                    Vector3 v1;
                    Vector3.Multiply(ref body.transformRate.Velocity, 1.0f - frac, out body.transformRate.Velocity);
                    Vector3.Multiply(ref vel, frac, out v1);
                    Vector3.Add(ref body.transformRate.Velocity, ref v1, out body.transformRate.Velocity);
                    #endregion
                }

                if (doAngVel)
                {
                    #region REFERENCE: body.AngVel = (frac * angVel + (1.0f - frac) * body.AngVel);
                    Vector3 v1;
                    Vector3.Multiply(ref body.transformRate.AngularVelocity, 1.0f - frac, out body.transformRate.AngularVelocity);
                    Vector3.Multiply(ref vel, frac, out v1);
                    Vector3.Add(ref body.transformRate.AngularVelocity, ref v1, out body.transformRate.AngularVelocity);
                    #endregion
                }
            }
            /// todo return false if we were already there...

            body.SetConstraintsAndCollisionsUnsatisfied();
            Satisfied = true;

            return(true);
        }
Esempio n. 25
0
        public void Run()
        {
            using (GameWindow game = new GameWindow(600, 400, GraphicsMode.Default, "3D Diffusion"))
            {
                game.Load += (sender, e) =>
                {
                    game.VSync = VSyncMode.On;
                };

                game.Resize += (sender, e) =>
                {
                    GL.Viewport(0, 0, game.Width, game.Height);
                };

                game.UpdateFrame += (sender, e) =>
                {
                    if (game.Keyboard[Key.Space])
                    {
                        liquidModel.SetState((uint)selectedX, (uint)selectedY, (uint)selectedZ, liquidModel.GetState((uint)selectedX, (uint)selectedY, (uint)selectedZ) + insertion);
                    }

                    liquidModel.Update(e.Time);
                };

                game.Keyboard.KeyDown += (sender, e) =>
                {
                    Key key = e.Key;
                    switch (key)
                    {
                    case Key.Escape:
                    {
                        game.Exit();
                        break;
                    }

                    case Key.W:
                    {
                        selectedX += 1;
                        if (selectedX >= liquidModel.Width)
                        {
                            selectedX = (int)liquidModel.Width - 1;
                        }
                        break;
                    }

                    case Key.S:
                    {
                        selectedX -= 1;
                        if (selectedX < 0)
                        {
                            selectedX = 0;
                        }
                        break;
                    }

                    case Key.E:
                    {
                        selectedY += 1;
                        if (selectedY >= liquidModel.Height)
                        {
                            selectedY = (int)liquidModel.Height - 1;
                        }
                        break;
                    }

                    case Key.Q:
                    {
                        selectedY -= 1;
                        if (selectedY < 0)
                        {
                            selectedY = 0;
                        }
                        break;
                    }

                    case Key.D:
                    {
                        selectedZ += 1;
                        if (selectedZ >= liquidModel.Depth)
                        {
                            selectedZ = (int)liquidModel.Depth - 1;
                        }
                        break;
                    }

                    case Key.A:
                    {
                        selectedZ -= 1;
                        if (selectedZ < 0)
                        {
                            selectedZ = 0;
                        }
                        break;
                    }
                    }
                };

                game.Mouse.Move += (sender, e) =>
                {
                    yaw   += ((float)Math.PI / 180.0f) * e.XDelta;
                    pitch += ((float)Math.PI / 180.0f) * e.YDelta;
                    if (pitch < -(float)Math.PI * 0.25f)
                    {
                        pitch = -(float)Math.PI * 0.25f;
                    }
                    else if (pitch > (float)Math.PI * 0.25f)
                    {
                        pitch = (float)Math.PI * 0.25f;
                    }
                };

                game.Mouse.WheelChanged += (sender, e) =>
                {
                    if (e.Delta > 0)
                    {
                        initialEyePosition = Vector3.Subtract(initialEyePosition, changeEyePosition);
                        if (initialEyePosition.Length < minEyePosition.Length)
                        {
                            initialEyePosition = minEyePosition;
                        }
                    }
                    else
                    {
                        initialEyePosition = Vector3.Add(initialEyePosition, changeEyePosition);
                        if (initialEyePosition.Length > maxEyePosition.Length)
                        {
                            initialEyePosition = maxEyePosition;
                        }
                    }
                };

                game.RenderFrame += (sender, e) =>
                {
                    if (game.Width < 1 || game.Height < 1)
                    {
                        return;
                    }
                    GL.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit);

                    GL.MatrixMode(MatrixMode.Projection);
                    Matrix4 perspective = Matrix4.CreatePerspectiveFieldOfView((float)Math.PI * 0.5f, (float)game.Width / (float)game.Height, 0.1f, 100.0f);
                    GL.LoadMatrix(ref perspective);

                    GL.MatrixMode(MatrixMode.Modelview);

                    Matrix3 cameraMoveMatrix = MatrixFromYawPitchRoll(yaw, pitch, roll);
                    Matrix4 lookAtMatrix     = Matrix4.LookAt(Vector3.Transform(initialEyePosition, cameraMoveMatrix), lookAtPosition, upVector);

                    GL.Enable(EnableCap.CullFace);
                    GL.CullFace(CullFaceMode.Back);
                    GL.FrontFace(FrontFaceDirection.Cw);

                    GL.Enable(EnableCap.Blend);
                    GL.BlendFunc(BlendingFactorSrc.SrcAlpha, BlendingFactorDest.OneMinusSrcAlpha);

                    GL.EnableClientState(ArrayCap.VertexArray);
                    GL.EnableClientState(ArrayCap.NormalArray);

                    GL.VertexPointer <float>(3, VertexPointerType.Float, 0, vertices);
                    GL.NormalPointer <float>(NormalPointerType.Float, 0, normals);


                    float   scaleX      = 1.0f / (float)liquidModel.Width;
                    float   scaleY      = 1.0f / (float)liquidModel.Height;
                    float   scaleZ      = 1.0f / (float)liquidModel.Depth;
                    Matrix4 scaleMatrix = Matrix4.CreateScale(scaleX, scaleY, scaleZ);

                    float minAlpha = 1.0f / (float)Math.Max(liquidModel.Width, Math.Max(liquidModel.Height, liquidModel.Depth));

                    for (uint i = 0; i < liquidModel.Width; i++)
                    {
                        for (uint j = 0; j < liquidModel.Height; j++)
                        {
                            for (uint k = 0; k < liquidModel.Depth; k++)
                            {
                                bool   isSelected = i == selectedX && j == selectedY && k == selectedZ;
                                Color4 color;
                                if (!isSelected)
                                {
                                    float value = liquidModel.GetState(i, j, k);
                                    color = Color4FromValue(value, minAlpha);
                                }
                                else
                                {
                                    color = Color4.White;
                                }
                                GL.Color4(color);

                                Matrix4 translateMatrix = Matrix4.CreateTranslation((float)i * scaleX, (float)j * scaleY, (float)k * scaleZ);
                                GL.LoadMatrix(ref lookAtMatrix);
                                GL.MultMatrix(ref initialTranslateMatrix);
                                GL.MultMatrix(ref translateMatrix);
                                GL.MultMatrix(ref scaleMatrix);
                                GL.DrawArrays(PrimitiveType.Triangles, 0, verticesCount);
                            }
                        }
                    }

                    GL.DisableClientState(ArrayCap.VertexArray);
                    GL.DisableClientState(ArrayCap.NormalArray);

                    GL.Disable(EnableCap.Blend);
                    GL.Disable(EnableCap.CullFace);

                    game.SwapBuffers();
                };

                game.Run(60.0);
            }
        }
Esempio n. 26
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)
        {
            Vector3 aAxisY, aAxisZ;
            Vector3 bAxisY;

            Matrix3x3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA);
            Matrix3x3.Transform(ref aLocalAxisY, ref connectionA.orientationMatrix, out aAxisY);
            Matrix3x3.Transform(ref aLocalAxisZ, ref connectionA.orientationMatrix, out aAxisZ);
            Matrix3x3.Transform(ref localAxisB, ref connectionB.orientationMatrix, out worldAxisB);
            Matrix3x3.Transform(ref bLocalAxisY, ref connectionB.orientationMatrix, out bAxisY);

            Quaternion rotation;

            Toolbox.GetQuaternionBetweenNormalizedVectors(ref worldAxisB, ref worldAxisA, out rotation);

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

            Vector3.Transform(ref bAxisY, 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 aAxisZ, out y);
            Vector3.Dot(ref twistMeasureAxis, ref aAxisY, out x);
            error = (float)Math.Atan2(y, x);

            //Debug.WriteLine("Angle: " + angle);

            //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.

            //Compute the jacobian.
            Vector3.Add(ref worldAxisA, ref worldAxisB, 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;

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

            springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness);
            biasVelocity = MathHelper.Clamp(-error * errorReduction, -maxCorrectiveVelocity, maxCorrectiveVelocity);

            //****** 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);
        }
Esempio n. 27
0
        bool TryToStepUsingContact(ref ContactData contact, out Vector3 newPosition)
        {
            Vector3 down     = character.Body.OrientationMatrix.Down;
            Vector3 position = character.Body.Position;
            //The normal of the contact may not be facing perfectly out to the side.
            //The detection process allows a bit of slop.
            //Correct it by removing any component of the normal along the local up vector.
            Vector3 normal = contact.Normal;
            float   dot;

            Vector3.Dot(ref normal, ref down, out dot);
            Vector3 error;

            Vector3.Multiply(ref down, dot, out error);
            Vector3.Subtract(ref normal, ref error, out normal);
            normal.Normalize();

            //Now we need to ray cast out from the center of the character in the direction of this normal to check for obstructions.
            //Compute the ray origin location.  Fire it out of the top of the character; if we're stepping, this must be a valid location.
            //Putting it as high as possible helps to reject more invalid step geometry.
            Ray   ray;
            float downRayLength = character.Body.Height;// MaximumStepHeight + upStepMargin;

            Vector3.Multiply(ref down, character.Body.Height * .5f - downRayLength, out ray.Position);
            Vector3.Add(ref ray.Position, ref position, out ray.Position);
            ray.Direction = normal;
            //Include a little margin in the length.
            //Technically, the character only needs to teleport horizontally by the complicated commented expression.
            //That puts it just far enough to have traction on the new surface.
            //In practice, the current contact refreshing approach used for many pair types causes contacts to persist horizontally a bit,
            //which can cause side effects for the character.
            float horizontalOffsetAmount = character.Body.CollisionInformation.Shape.CollisionMargin; // (float)((1 - character.SupportFinder.sinMaximumSlope) * character.Body.CollisionInformation.Shape.CollisionMargin + 0);
            float length = character.Body.Radius + horizontalOffsetAmount;                            // -contact.PenetrationDepth;


            if (character.QueryManager.RayCastHitAnything(ray, length))
            {
                //The step is obstructed!
                newPosition = new Vector3();
                return(false);
            }

            //The down-cast ray origin has been verified by the previous ray cast.
            //Let's look for a support!
            Vector3 horizontalOffset;

            Vector3.Multiply(ref normal, length, out horizontalOffset);
            Vector3.Add(ref ray.Position, ref horizontalOffset, out ray.Position);
            ray.Direction = down;

            //Find the earliest hit, if any.
            RayHit earliestHit = new RayHit();

            if (!character.QueryManager.RayCast(ray, downRayLength, out earliestHit) || //Can't do anything if it didn't hit.
                earliestHit.T <= 0 ||                                                   //Can't do anything if the hit was invalid.
                earliestHit.T - downRayLength > -minimumUpStepHeight ||                 //Don't bother doing anything if the step is too small.
                earliestHit.T - downRayLength < -maximumStepHeight - upStepMargin)      //Can't do anything if the step is too tall.
            {
                //No valid hit was detected.
                newPosition = new Vector3();
                return(false);
            }

            //Ensure the candidate surface supports traction.
            Vector3 supportNormal;

            Vector3.Normalize(ref earliestHit.Normal, out supportNormal);
            //Calibrate the normal to face in the same direction as the down vector for consistency.
            Vector3.Dot(ref supportNormal, ref down, out dot);
            if (dot < 0)
            {
                Vector3.Negate(ref supportNormal, out supportNormal);
                dot = -dot;
            }

            //If the new surface does not have traction, do not attempt to step up.
            if (dot < character.SupportFinder.cosMaximumSlope)
            {
                newPosition = new Vector3();
                return(false);
            }

            //Since contact queries are frequently expensive compared to ray cast tests,
            //do one more ray cast test.  This time, starting from the same position, cast upwards.
            //In order to step up, the previous down-ray hit must be at least a character height away from the result of the up-ray.
            Vector3.Negate(ref down, out ray.Direction);
            //Find the earliest hit, if any.
            //RayHit earliestHitUp = new RayHit();
            //earliestHitUp.T = float.MaxValue;
            float upLength = character.Body.Height - earliestHit.T;

            //If the sum of the up and down distances is less than the height, the character can't fit.
            if (character.QueryManager.RayCastHitAnything(ray, upLength))
            {
                newPosition = new Vector3();
                return(false);
            }

            //By now, a valid ray hit has been found.  Now we need to validate it using contact queries.
            //This process is very similar in concept to the down step verification, but it has some extra
            //requirements.

            //Predict a hit location based on the time of impact and the normal at the intersection.
            //Take into account the radius of the character (don't forget the collision margin!)



            RigidTransform transform = character.Body.CollisionInformation.WorldTransform;

            //The transform must be modified to position the query body at the right location.
            //The horizontal offset of the queries ensures that a tractionable part of the character will be put onto the new support.
            Vector3.Multiply(ref normal, horizontalOffsetAmount, out horizontalOffset);
            Vector3.Add(ref transform.Position, ref horizontalOffset, out transform.Position);
            Vector3 verticalOffset;

            Vector3.Multiply(ref down, -downRayLength, out verticalOffset);
            Vector3.Add(ref transform.Position, ref verticalOffset, out transform.Position);

            //We know that the closest point to the plane will be the extreme point in the plane's direction.
            //Use it as the ray origin.
            Ray downRay;

            character.Body.CollisionInformation.Shape.GetExtremePoint(supportNormal, ref transform, out downRay.Position);
            downRay.Direction = down;

            //Intersect the ray against the plane defined by the support hit.
            Vector3 intersection;

            Vector3.Dot(ref earliestHit.Location, ref supportNormal, out dot);
            Plane   plane = new Plane(supportNormal, dot);
            Vector3 candidatePosition;

            //Define the interval bounds to be used later.

            //The words 'highest' and 'lowest' here refer to the position relative to the character's body.
            //The ray cast points downward relative to the character's body.
            float highestBound  = -maximumStepHeight;
            float lowestBound   = character.Body.CollisionInformation.Shape.CollisionMargin - downRayLength + earliestHit.T;
            float currentOffset = lowestBound;
            float hintOffset;



            //This guess may either win immediately, or at least give us a better idea of where to search.
            float hitT;

            if (Toolbox.GetRayPlaneIntersection(ref downRay, ref plane, out hitT, out intersection))
            {
                hitT = -downRayLength + hitT + CollisionDetectionSettings.AllowedPenetration;
                if (hitT < highestBound)
                {
                    //Don't try a location known to be too high.
                    hitT = highestBound;
                }
                currentOffset = hitT;
                if (currentOffset > lowestBound)
                {
                    lowestBound = currentOffset;
                }
                candidatePosition = character.Body.Position + down * currentOffset + horizontalOffset;
                switch (TryUpStepPosition(ref normal, ref candidatePosition, out hintOffset))
                {
                case PositionState.Accepted:
                    currentOffset += hintOffset;
                    //Only use the new position location if the movement distance was the right size.
                    if (currentOffset < 0 && currentOffset > -maximumStepHeight - CollisionDetectionSettings.AllowedPenetration)
                    {
                        //It's possible that we let a just-barely-too-high step occur, limited by the allowed penetration.
                        //Just clamp the overall motion and let it penetrate a bit.
                        newPosition = character.Body.Position + Math.Max(-maximumStepHeight, currentOffset) * down + horizontalOffset;
                        return(true);
                    }
                    else
                    {
                        newPosition = new Vector3();
                        return(false);
                    }

                case PositionState.Rejected:
                    newPosition = new Vector3();
                    return(false);

                case PositionState.NoHit:
                    highestBound  = currentOffset + hintOffset;
                    currentOffset = (lowestBound + currentOffset) * .5f;
                    break;

                case PositionState.Obstructed:
                    lowestBound   = currentOffset;
                    currentOffset = (highestBound + currentOffset) * .5f;
                    break;

                case PositionState.HeadObstructed:
                    highestBound  = currentOffset + hintOffset;
                    currentOffset = (lowestBound + currentOffset) * .5f;
                    break;

                case PositionState.TooDeep:
                    currentOffset += hintOffset;
                    lowestBound    = currentOffset;
                    break;
                }
            }//TODO: If the ray cast doesn't hit, that could be used to early out...  Then again, it pretty much can't happen.

            //Our guesses failed.
            //Begin the regular process.  Start at the time of impact of the ray itself.
            //How about trying the time of impact of the ray itself?

            //Since we wouldn't be here unless there were no contacts at the body's current position,
            //testing the ray cast location gives us the second bound we need to do an informed binary search.



            int attempts = 0;

            //Don't keep querying indefinitely.  If we fail to reach it in a few informed steps, it's probably not worth continuing.
            //The bound size check prevents the system from continuing to search a meaninglessly tiny interval.
            while (attempts++ < 5 && lowestBound - highestBound > Toolbox.BigEpsilon)
            {
                candidatePosition = character.Body.Position + currentOffset * down + horizontalOffset;
                switch (TryUpStepPosition(ref normal, ref candidatePosition, out hintOffset))
                {
                case PositionState.Accepted:
                    currentOffset += hintOffset;
                    //Only use the new position location if the movement distance was the right size.
                    if (currentOffset < 0 && currentOffset > -maximumStepHeight - CollisionDetectionSettings.AllowedPenetration)
                    {
                        //It's possible that we let a just-barely-too-high step occur, limited by the allowed penetration.
                        //Just clamp the overall motion and let it penetrate a bit.
                        newPosition = character.Body.Position + Math.Max(-maximumStepHeight, currentOffset) * down + horizontalOffset;
                        return(true);
                    }
                    else
                    {
                        newPosition = new Vector3();
                        return(false);
                    }

                case PositionState.Rejected:
                    newPosition = new Vector3();
                    return(false);

                case PositionState.NoHit:
                    highestBound  = currentOffset + hintOffset;
                    currentOffset = (lowestBound + highestBound) * .5f;
                    break;

                case PositionState.Obstructed:
                    lowestBound   = currentOffset;
                    currentOffset = (highestBound + lowestBound) * .5f;
                    break;

                case PositionState.HeadObstructed:
                    highestBound  = currentOffset + hintOffset;
                    currentOffset = (lowestBound + currentOffset) * .5f;
                    break;

                case PositionState.TooDeep:
                    currentOffset += hintOffset;
                    lowestBound    = currentOffset;
                    break;
                }
            }
            //Couldn't find a candidate.
            newPosition = new Vector3();
            return(false);
        }
Esempio n. 28
0
        public bool RayCast(Ray ray, float maximumLength, IList <BroadPhaseEntry> outputIntersections)
        {
            if (maximumLength == float.MaxValue)
            {
                throw new NotSupportedException(
                          "The Grid2DSortAndSweep broad phase cannot accelerate infinite ray casts.  Consider specifying a maximum length or using a broad phase which supports infinite ray casts.");
            }

            //Use 2d line rasterization.
            //Compute the exit location in the cell.
            //Test against each bounding box up until the exit value is reached.
            float   length = 0;
            Int2    cellIndex;
            Vector3 currentPosition = ray.Position;

            Grid2DSortAndSweep.ComputeCell(ref currentPosition, out cellIndex);
            while (true)
            {
                float cellWidth = 1 / Grid2DSortAndSweep.cellSizeInverse;
                float nextT;  //Distance along ray to next boundary.
                float nextTy; //Distance along ray to next boundary along y axis.
                float nextTz; //Distance along ray to next boundary along z axis.
                //Find the next cell.
                if (ray.Direction.Y > 0)
                {
                    nextTy = ((cellIndex.Y + 1) * cellWidth - currentPosition.Y) / ray.Direction.Y;
                }
                else if (ray.Direction.Y < 0)
                {
                    nextTy = (cellIndex.Y * cellWidth - currentPosition.Y) / ray.Direction.Y;
                }
                else
                {
                    nextTy = 10e10f;
                }

                if (ray.Direction.Z > 0)
                {
                    nextTz = ((cellIndex.Z + 1) * cellWidth - currentPosition.Z) / ray.Direction.Z;
                }
                else if (ray.Direction.Z < 0)
                {
                    nextTz = (cellIndex.Z * cellWidth - currentPosition.Z) / ray.Direction.Z;
                }
                else
                {
                    nextTz = 10e10f;
                }

                bool yIsMinimum = nextTy < nextTz;
                nextT = yIsMinimum ? nextTy : nextTz;


                //Grab the cell that we are currently in.
                GridCell2D cell;
                if (owner.cellSet.TryGetCell(ref cellIndex, out cell))
                {
                    float endingX;
                    if (ray.Direction.X < 0)
                    {
                        endingX = currentPosition.X;
                    }
                    else
                    {
                        endingX = currentPosition.X + ray.Direction.X * nextT;
                    }

                    //To fully accelerate this, the entries list would need to contain both min and max interval markers.
                    //Since it only contains the sorted min intervals, we can't just start at a point in the middle of the list.
                    //Consider some giant bounding box that spans the entire list.
                    for (int i = 0;
                         i < cell.entries.Count &&
                         cell.entries.Elements[i].item.boundingBox.Min.X <= endingX;
                         i++) //TODO: Try additional x axis pruning?
                    {
                        BroadPhaseEntry item = cell.entries.Elements[i].item;
                        float           t;
                        if (ray.Intersects(ref item.boundingBox, out t) && t < maximumLength &&
                            !outputIntersections.Contains(item))
                        {
                            outputIntersections.Add(item);
                        }
                    }
                }

                //Move the position forward.
                length += nextT;
                if (length > maximumLength
                    ) //Note that this catches the case in which the ray is pointing right down the middle of a row (resulting in a nextT of 10e10f).
                {
                    break;
                }

                Vector3 offset;
                Vector3.Multiply(ref ray.Direction, nextT, out offset);
                Vector3.Add(ref offset, ref currentPosition, out currentPosition);
                if (yIsMinimum)
                {
                    if (ray.Direction.Y < 0)
                    {
                        cellIndex.Y -= 1;
                    }
                    else
                    {
                        cellIndex.Y += 1;
                    }
                }
                else if (ray.Direction.Z < 0)
                {
                    cellIndex.Z -= 1;
                }
                else
                {
                    cellIndex.Z += 1;
                }
            }

            return(outputIntersections.Count > 0);
        }
 public override void Transform(ref Vector3 v, out Vector3 res)
 {
     Vector3.Add(ref v, ref _offset, out res);
 }
Esempio n. 30
0
        public DrawingSurface Render(VxlFile vxl, HvaFile hva, GameObject obj, DrawProperties props)
        {
            if (!_isInit)
            {
                Initialize();
            }
            if (!_canRender)
            {
                Logger.Warn("Not rendering {0} because no OpenGL context could be obtained", vxl.FileName);
                return(null);
            }

            Logger.Debug("Rendering voxel {0}", vxl.FileName);
            vxl.Initialize();
            hva.Initialize();

            GL.Viewport(0, 0, _surface.BitmapData.Width, _surface.BitmapData.Height);
            GL.Clear(ClearBufferMask.DepthBufferBit | ClearBufferMask.ColorBufferBit);

            // RA2 uses dimetric projection with camera elevated 30° off the ground
            GL.MatrixMode(MatrixMode.Projection);
            var persp = Matrix4.CreatePerspectiveFieldOfView(MathHelper.DegreesToRadians(30), _surface.BitmapData.Width / (float)_surface.BitmapData.Height, 1, _surface.BitmapData.Height);

            GL.LoadMatrix(ref persp);

            GL.MatrixMode(MatrixMode.Modelview);
            GL.LoadIdentity();

            var lookat = Matrix4.LookAt(0, 0, -10, 0, 0, 0, 0, 1, 0);

            GL.MultMatrix(ref lookat);

            var trans = Matrix4.CreateTranslation(0, 0, 10);

            GL.MultMatrix(ref trans);

            // align and zoom
            var world = Matrix4.CreateRotationX(MathHelper.DegreesToRadians(60));

            world = Matrix4.CreateRotationY(MathHelper.DegreesToRadians(180)) * world;
            world = Matrix4.CreateRotationZ(MathHelper.DegreesToRadians(-45)) * world;
            world = Matrix4.CreateScale(0.028f, 0.028f, 0.028f) * world;
            GL.MultMatrix(ref world);

            // DrawAxes();

            // determine tilt vectors
            Matrix4 tilt = Matrix4.Identity;
            int     tiltPitch = 0, tiltYaw = 0;

            if (obj.Tile.Drawable != null)
            {
                var img  = (obj.Tile.Drawable as TileDrawable).GetTileImage(obj.Tile);
                int ramp = img?.RampType ?? 0;
                if (ramp == 0 || ramp >= 17)
                {
                    tiltPitch = tiltYaw = 0;
                }
                else if (ramp <= 4)
                {
                    // screen-diagonal facings (perpendicular to axes)
                    tiltPitch = 25;
                    tiltYaw   = -90 * ramp;
                }
                else
                {
                    // world-diagonal facings (perpendicular to screen)
                    tiltPitch = 25;
                    tiltYaw   = 225 - 90 * ((ramp - 1) % 4);
                }
                tilt *= Matrix4.CreateRotationX(MathHelper.DegreesToRadians(tiltPitch));
                tilt *= Matrix4.CreateRotationZ(MathHelper.DegreesToRadians(tiltYaw));

                /*// show tilt direction
                 *              GL.Color3(Color.Black);
                 *              GL.Begin(BeginMode.Lines);
                 *              GL.Vertex3(Vector3.Zero);
                 *              var tiltVec = Vector3.UnitZ;
                 *              tiltVec = Vector3.Transform(tiltVec, tilt);
                 *              tiltVec = Vector3.Multiply(tiltVec, 1000f);
                 *              GL.Vertex3(tiltVec);
                 *              GL.End();*/
            }

            /*// draw slope normals
             *          GL.LineWidth(2);
             *          var colors = new[] { Color.Red, Color.Green, Color.Blue, Color.Yellow, Color.Orange, Color.Black, Color.Purple, Color.SlateBlue, Color.DimGray, Color.White, Color.Teal, Color.Tan };
             *          for (int i = 0; i < 8; i++) {
             *                  GL.Color3(colors[i]);
             *
             *                  const float roll = 25f;
             *                  float syaw = 45f * i;
             *                  var slopeNormal = Vector3.UnitZ;
             *                  slopeNormal = Vector3.Transform(slopeNormal, Matrix4.CreateRotationX(MathHelper.DegreesToRadians(roll)));
             *                  slopeNormal = Vector3.Transform(slopeNormal, Matrix4.CreateRotationZ(MathHelper.DegreesToRadians(syaw)));
             *                  GL.Begin(BeginMode.Lines);
             *                  GL.Vertex3(0, 0, 0);
             *                  GL.Vertex3(Vector3.Multiply(slopeNormal, 1000f));
             *                  GL.End();
             *          }*/


            // object rotation around Z
            float   direction      = (obj is OwnableObject) ? (obj as OwnableObject).Direction : 0;
            float   objectRotation = 90 - direction / 256f * 360f - tiltYaw;                                      // convert game rotation to world degrees
            Matrix4 @object        = Matrix4.CreateRotationZ(MathHelper.DegreesToRadians(objectRotation)) * tilt; // object facing

            // art.ini TurretOffset value positions some voxel parts over our x-axis
            @object = Matrix4.CreateTranslation(0.18f * props.TurretVoxelOffset, 0, 0) * @object;
            GL.MultMatrix(ref @object);

            // DrawAxes();

            float pitch = MathHelper.DegreesToRadians(210);
            float yaw   = MathHelper.DegreesToRadians(120);

            /*// helps to find good pitch/yaw
             *          // direction of light vector given by pitch & yaw
             *          for (int i = 0; i < 360; i += 30) {
             *                  for (int j = 0; j < 360; j += 30) {
             *                          GL.Color3(colors[i / 30]);
             *                          var shadowTransform2 =
             *                                  Matrix4.CreateRotationZ(MathHelper.DegreesToRadians(i))
             * Matrix4.CreateRotationY(MathHelper.DegreesToRadians(j));
             *                          GL.LineWidth(2);
             *                          GL.Begin(BeginMode.Lines);
             *                          GL.Vertex3(0, 0, 0);
             *                          GL.Vertex3(Vector3.Multiply(ExtractRotationVector(ToOpenGL(Matrix4.Invert(world * shadowTransform2))), 100f));
             *                          GL.End();
             *                  }
             *          }*/

            var shadowTransform = Matrix4.CreateRotationZ(pitch) * Matrix4.CreateRotationY(yaw);
            // clear shadowbuf
            var shadBuf = _surface.GetShadows();

            Array.Clear(shadBuf, 0, shadBuf.Length);

            foreach (var section in vxl.Sections)
            {
                GL.PushMatrix();

                var frameRot = hva.LoadGLMatrix(section.Index);
                frameRot.M41 *= section.HVAMultiplier * section.ScaleX;
                frameRot.M42 *= section.HVAMultiplier * section.ScaleY;
                frameRot.M43 *= section.HVAMultiplier * section.ScaleZ;

                var frameTransl = Matrix4.CreateTranslation(section.MinBounds);
                var frame       = frameTransl * frameRot;
                GL.MultMatrix(ref frame);

                var shadowScale = Matrix4.CreateScale(0.5f);
                //var shadowTilt = null;
                var shadowToScreen = frameTransl * shadowScale * frameRot * (@object * world) * trans * lookat;

                // undo world transformations on light direction
                var v = @object * world * frame * shadowTransform;

                var lightDirection = (v.Determinant != 0.0) ? ExtractRotationVector(ToOpenGL(Matrix4.Invert(v))) : Vector3.Zero;

                // draw line in direction light comes from

                /*GL.Color3(Color.Red);
                 *              GL.LineWidth(4f);
                 *              GL.Begin(BeginMode.Lines);
                 *              GL.Vertex3(0, 0, 0);
                 *              GL.Vertex3(Vector3.Multiply(lightDirection, 100f));
                 *              GL.End();*/

                GL.Begin(PrimitiveType.Quads);
                for (uint x = 0; x != section.SizeX; x++)
                {
                    for (uint y = 0; y != section.SizeY; y++)
                    {
                        foreach (VxlFile.Voxel vx in section.Spans[x, y].Voxels)
                        {
                            if (vx.ColorIndex == 0)
                            {
                                continue;
                            }
                            Color   color  = obj.Palette.Colors[vx.ColorIndex];
                            Vector3 normal = section.GetNormal(vx.NormalIndex);
                            // shader function taken from https://github.com/OpenRA/OpenRA/blob/bleed/cg/vxl.fx
                            // thanks to pchote for a LOT of help getting it right
                            Vector3 colorMult = Vector3.Add(Ambient, Diffuse * Math.Max(Vector3.Dot(normal, lightDirection), 0f));
                            GL.Color3(
                                (byte)Math.Min(255, color.R * colorMult.X),
                                (byte)Math.Min(255, color.G * colorMult.Y),
                                (byte)Math.Min(255, color.B * colorMult.Z));

                            Vector3 vxlPos = Vector3.Multiply(new Vector3(x, y, vx.Z), section.Scale);
                            RenderVoxel(vxlPos);

                            var shadpos   = new Vector3(x, y, 0);
                            var screenPos = Vector3.TransformVector(shadpos, shadowToScreen);
                            screenPos    = Vector3.TransformVector(screenPos, persp);
                            screenPos.X /= screenPos.Z;
                            screenPos.Y /= screenPos.Z;
                            screenPos.X  = (screenPos.X + 1) * _surface.Width / 2;
                            screenPos.Y  = (screenPos.Y + 1) * _surface.Height / 2;

                            if (0 <= screenPos.X && screenPos.X < _surface.Width && 0 <= screenPos.Y && screenPos.Y < _surface.Height)
                            {
                                shadBuf[(int)screenPos.X + (_surface.Height - 1 - (int)screenPos.Y) * _surface.Width] = true;
                            }

                            /* draw line in normal direction
                             *                          if (r.Next(100) == 4) {
                             *                                  float m = Math.Max(Vector3.Dot(normal, lightDirection), 0f);
                             *                                  GL.Color3(m, m, m);
                             *                                  GL.LineWidth(1);
                             *                                  GL.Begin(BeginMode.Lines);
                             *                                  GL.Vertex3(new Vector3(x, y, vx.Z));
                             *                                  GL.Vertex3(new Vector3(x, y, vx.Z) + Vector3.Multiply(normal, 100f));
                             *                                  GL.End();
                             *                          }*/
                        }
                    }
                }
                GL.End();
                GL.PopMatrix();
            }

            // read pixels back to surface
            GL.ReadPixels(0, 0, _surface.BitmapData.Width, _surface.BitmapData.Height, OpenTK.Graphics.OpenGL.PixelFormat.Bgra, PixelType.UnsignedByte, _surface.BitmapData.Scan0);
            return(_surface);
        }
Esempio n. 31
0
        public static unsafe void Blend(AnimationBlendOperation blendOperation, float blendFactor, AnimationClipResult sourceLeft, AnimationClipResult sourceRight, AnimationClipResult result)
        {
            fixed(byte *sourceLeftDataStart = sourceLeft.Data)
            fixed(byte *sourceRightDataStart = sourceRight.Data)
            fixed(byte *resultDataStart      = result.Data)
            {
                foreach (var channel in sourceLeft.Channels)
                {
                    int offset          = channel.Offset;
                    var sourceLeftData  = (float *)(sourceLeftDataStart + offset);
                    var sourceRightData = (float *)(sourceRightDataStart + offset);
                    var resultData      = (float *)(resultDataStart + offset);

                    float factorLeft  = *sourceLeftData++;
                    float factorRight = *sourceRightData++;

                    // Ignore this channel
                    if (factorLeft == 0.0f && factorRight == 0.0f)
                    {
                        *resultData++ = 0.0f;
                        continue;
                    }

                    // Use left value
                    if (factorLeft > 0.0f && factorRight == 0.0f)
                    {
                        *resultData++ = 1.0f;
                        Utilities.CopyMemory((IntPtr)resultData, (IntPtr)sourceLeftData, channel.Size);
                        continue;
                    }

                    // Use right value
                    if (factorRight > 0.0f && factorLeft == 0.0f)
                    {
                        *resultData++ = 1.0f;
                        Utilities.CopyMemory((IntPtr)resultData, (IntPtr)sourceRightData, channel.Size);
                        continue;
                    }

                    // Blending
                    *resultData++ = 1.0f;

                    switch (blendOperation)
                    {
                    case AnimationBlendOperation.LinearBlend:
                        // Perform linear blending
                        // It will blend between left (0.0) and right (1.0)
                        switch (channel.BlendType)
                        {
                        case BlendType.None:
                            Utilities.CopyMemory((IntPtr)resultData, (IntPtr)sourceLeftData, channel.Size);
                            break;

                        case BlendType.Float2:
                            Vector2.Lerp(ref *(Vector2 *)sourceLeftData, ref *(Vector2 *)sourceRightData, blendFactor, out *(Vector2 *)resultData);
                            break;

                        case BlendType.Float3:
                            Vector3.Lerp(ref *(Vector3 *)sourceLeftData, ref *(Vector3 *)sourceRightData, blendFactor, out *(Vector3 *)resultData);
                            break;

                        case BlendType.Quaternion:
                            Quaternion.Slerp(ref *(Quaternion *)sourceLeftData, ref *(Quaternion *)sourceRightData, blendFactor, out *(Quaternion *)resultData);
                            break;

                        default:
                            throw new ArgumentOutOfRangeException();
                        }
                        break;

                    case AnimationBlendOperation.Add:
                        // Perform additive blending
                        // It will blend between left (0.0) and left + right (1.0).
                        switch (channel.BlendType)
                        {
                        case BlendType.None:
                            Utilities.CopyMemory((IntPtr)resultData, (IntPtr)sourceLeftData, channel.Size);
                            break;

                        case BlendType.Float2:
                            Vector2 rightValue2;
                            Vector2.Add(ref *(Vector2 *)sourceLeftData, ref *(Vector2 *)sourceRightData, out rightValue2);
                            Vector2.Lerp(ref *(Vector2 *)sourceLeftData, ref rightValue2, blendFactor, out *(Vector2 *)resultData);
                            break;

                        case BlendType.Float3:
                            Vector3 rightValue3;
                            Vector3.Add(ref *(Vector3 *)sourceLeftData, ref *(Vector3 *)sourceRightData, out rightValue3);
                            Vector3.Lerp(ref *(Vector3 *)sourceLeftData, ref rightValue3, blendFactor, out *(Vector3 *)resultData);
                            break;

                        case BlendType.Quaternion:
                            Quaternion rightValueQ;
                            Quaternion.Multiply(ref *(Quaternion *)sourceLeftData, ref *(Quaternion *)sourceRightData, out rightValueQ);
                            Quaternion.Normalize(ref rightValueQ, out rightValueQ);
                            Quaternion.Slerp(ref *(Quaternion *)sourceLeftData, ref rightValueQ, blendFactor, out *(Quaternion *)resultData);
                            break;

                        default:
                            throw new ArgumentOutOfRangeException();
                        }
                        break;

                    case AnimationBlendOperation.Subtract:
                        // Perform subtractive blending
                        // It will blend between left (0.0) and left - right (1.0).
                        switch (channel.BlendType)
                        {
                        case BlendType.None:
                            Utilities.CopyMemory((IntPtr)resultData, (IntPtr)sourceLeftData, channel.Size);
                            break;

                        case BlendType.Float2:
                            Vector2 rightValue2;
                            Vector2.Subtract(ref *(Vector2 *)sourceLeftData, ref *(Vector2 *)sourceRightData, out rightValue2);
                            Vector2.Lerp(ref *(Vector2 *)sourceLeftData, ref rightValue2, blendFactor, out *(Vector2 *)resultData);
                            break;

                        case BlendType.Float3:
                            Vector3 rightValue3;
                            Vector3.Subtract(ref *(Vector3 *)sourceLeftData, ref *(Vector3 *)sourceRightData, out rightValue3);
                            Vector3.Lerp(ref *(Vector3 *)sourceLeftData, ref rightValue3, blendFactor, out *(Vector3 *)resultData);
                            break;

                        case BlendType.Quaternion:
                            Quaternion rightValueQ;
                            // blend between left (0.0) and left * conjugate(right) (1.0)
                            Quaternion.Invert(ref *(Quaternion *)sourceRightData, out rightValueQ);
                            Quaternion.Multiply(ref rightValueQ, ref *(Quaternion *)sourceLeftData, out rightValueQ);
                            Quaternion.Normalize(ref rightValueQ, out rightValueQ);
                            //throw new NotImplementedException();
                            Quaternion.Slerp(ref *(Quaternion *)sourceLeftData, ref rightValueQ, blendFactor, out *(Quaternion *)resultData);
                            break;

                        default:
                            throw new ArgumentOutOfRangeException();
                        }
                        break;

                    default:
                        throw new ArgumentOutOfRangeException("blendOperation");
                    }
                }
            }
        }
Esempio n. 32
0
        /// <summary>
        ///     Actualiza el BoundingBox de la caja.
        ///     No contempla rotacion
        /// </summary>
        private void updateBoundingBox()
        {
            var midSize = Vector3.Scale(size, 0.5f);

            BoundingBox.setExtremes(Vector3.Subtract(translation, midSize), Vector3.Add(translation, midSize));
        }
Esempio n. 33
0
        private static void ProcessKeys()
        {
            var     pos     = Vector3.Zero;
            var     up      = Vector3.UnitY;
            var     right   = Vector3.UnitX;
            Vector3 forward = Vector3.UnitZ;
            Vector3 upDir;
            Vector3 rightDir;
            Vector3 lookAt;
            Matrix  rotMatrix;
            Vector3 cameraDir = Vector3.Zero;

            if (_keyboard.KeyStates[KeyboardKeys.Left] == KeyState.Down)
            {
                _cameraRotation.X -= 40.0f * GorgonTiming.Delta;
            }
            else
            if (_keyboard.KeyStates[KeyboardKeys.Right] == KeyState.Down)
            {
                _cameraRotation.X += 40.0f * GorgonTiming.Delta;
            }
            else
            if (_keyboard.KeyStates[KeyboardKeys.Up] == KeyState.Down)
            {
                _cameraRotation.Y -= 40.0f * GorgonTiming.Delta;
            }
            else
            if (_keyboard.KeyStates[KeyboardKeys.Down] == KeyState.Down)
            {
                _cameraRotation.Y += 40.0f * GorgonTiming.Delta;
            }
            else if (_keyboard.KeyStates[KeyboardKeys.PageUp] == KeyState.Down)
            {
                _cameraRotation.Z -= 40.0f * GorgonTiming.Delta;
            }
            else if (_keyboard.KeyStates[KeyboardKeys.PageDown] == KeyState.Down)
            {
                _cameraRotation.Z += 40.0f * GorgonTiming.Delta;
            }
            else if (_keyboard.KeyStates[KeyboardKeys.D] == KeyState.Down)
            {
                cameraDir.X = 2.0f * GorgonTiming.Delta;
            }
            else if (_keyboard.KeyStates[KeyboardKeys.A] == KeyState.Down)
            {
                cameraDir.X = -2.0f * GorgonTiming.Delta;
            }
            else if (_keyboard.KeyStates[KeyboardKeys.W] == KeyState.Down)
            {
                cameraDir.Z = 2.0f * GorgonTiming.Delta;
            }
            else if (_keyboard.KeyStates[KeyboardKeys.S] == KeyState.Down)
            {
                cameraDir.Z = -2.0f * GorgonTiming.Delta;
            }
            else if (_keyboard.KeyStates[KeyboardKeys.Q] == KeyState.Down)
            {
                cameraDir.Y = 2.0f * GorgonTiming.Delta;
            }
            else if (_keyboard.KeyStates[KeyboardKeys.E] == KeyState.Down)
            {
                cameraDir.Y = -2.0f * GorgonTiming.Delta;
            }

            if (!_lock)
            {
                Matrix.RotationYawPitchRoll(_cameraRotation.X.Radians(),
                                            _cameraRotation.Y.Radians(),
                                            _cameraRotation.Z.Radians(),
                                            out rotMatrix);
                Vector3.TransformCoordinate(ref forward, ref rotMatrix, out lookAt);
                Vector3.TransformCoordinate(ref right, ref rotMatrix, out rightDir);
                Vector3.Cross(ref lookAt, ref rightDir, out upDir);
            }
            else
            {
                upDir    = up;
                rightDir = right;
                lookAt   = _sphere.Position;
            }

            Vector3.Multiply(ref rightDir, cameraDir.X, out rightDir);
            Vector3.Add(ref _cameraPosition, ref rightDir, out _cameraPosition);
            Vector3.Multiply(ref lookAt, cameraDir.Z, out forward);
            Vector3.Add(ref _cameraPosition, ref forward, out _cameraPosition);
            Vector3.Multiply(ref upDir, cameraDir.Y, out up);
            Vector3.Add(ref _cameraPosition, ref up, out _cameraPosition);

            //lookAt.Normalize();
            if (!_lock)
            {
                Vector3.Add(ref _cameraPosition, ref lookAt, out lookAt);
            }

            _wvp.UpdateViewMatrix(ref _cameraPosition, ref lookAt, ref upDir);
        }
        //public Vector3 Radiance(Ray ray, int stop)
        public Vector3 Radiance(Ray ray, int stop, Tree arbre)
        {
            Vector3 color     = new Vector3(0, 0, 0);
            float   nb_rebond = 10.0f;

            //tant qu'on a pas eu 100 rebonds
            if (stop != 10)
            {
                //recherche d'intersection pour sphere
                //ResIntersect resInter = Intersects(ray);

                //recherche d'intersection pour sphere AVEC structure acceleratrice
                ResIntersect resInter = IntersectTree(arbre, ray);


                //si on a une intersection
                if (resInter.t != -1)
                {
                    //interpoint, coordonnées du point t'intersection
                    Vector3 interPoint = Vector3.Add(ray.point, Vector3.Multiply(ray.direction, resInter.t));
                    //normal, vecteur normal a la surface de la sphere
                    Vector3 normal = Vector3.Subtract(interPoint, resInter.sph.center);
                    normal = Vector3.Normalize(normal);
                    //ajout du petit décalage pour ne pas avoir d'interesection d'un point d'une sphere avec un autre point de la sphere
                    interPoint = Vector3.Add(interPoint, Vector3.Multiply(normal, 0.1f));

                    //selon le materiau
                    switch (resInter.sph.material.type)
                    {
                    //DIFFUS
                    case MaterialType.Difuse:
                        //directionToLight, vecteur poitant vers la lumiere à partir du point d'intersection sur la sphere
                        Vector3 directionToLight = Vector3.Subtract(light.origin, interPoint);
                        //création d'un Rayon à partir de cette direction
                        ray = new Ray(interPoint, directionToLight);
                        //cherche s'il y a un obstacle entre la lumiere et le point de la sphere
                        ResIntersect resInterL = Intersects(ray);
                        //lightPower contiendra les albedo additionnés à chaque rebond
                        Vector3 lightPower = new Vector3(0.0f, 0.0f, 0.0f);

                        //s'il n'y a pas d'obstacle et s'il ne s'agit pas de la lampe
                        if (!(resInterL.t != -1 && resInterL.t <= 1.0f && resInterL.sph.material.type != MaterialType.Light))
                        {
                            lightPower = ReceiveLight(light, interPoint, resInter.sph);
                        }

                        //calcul des coordonnées d'un point d'une sphere fictive sur le point d'intersection de la shpere pour calculer une nouvelle direction
                        Vector3 mini_sphere_coord = RandomVector(interPoint);

                        //création d'une base à partir du vecteur normal au point d'intersection de la sphere
                        Vector3 randVect = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
                        Vector3 xBase    = Vector3.Cross(normal, randVect);
                        Vector3 yBase    = Vector3.Cross(normal, xBase);
                        Vector3 zBase    = normal;

                        //calcul d'une nouvelle direction grace à la nouvelle base
                        Vector3 newDirect       = Vector3.Add(Vector3.Add(xBase * mini_sphere_coord.X, yBase * mini_sphere_coord.Y), zBase * mini_sphere_coord.Z);
                        Vector3 normalNewDirect = Vector3.Normalize(newDirect);


                        //dans la generation de la direction aléatoire du rebond, pour ne pas avoir de vecteur qui va dans la sphere
                        if (Vector3.Dot(normalNewDirect, normal) < 0)
                        {
                            newDirect = -newDirect;
                        }

                        //création d'un nouveau rayon faire le prochain rebond
                        Ray rebond = new Ray(interPoint, newDirect);
                        //cumul des albedo de toutes les surfaces sur lesquelles on rebondit
                        color = Vector3.Add(lightPower, Vector3.Multiply(Vector3.Dot(normalNewDirect, normal), Vector3.Add(resInter.sph.material.albedo, Radiance(rebond, ++stop, arbre))));


                        //division du cumul d'albedo par le nombre de rebond
                        color = Vector3.Divide(color, nb_rebond);

                        break;


                    //MIRROIR
                    case MaterialType.Mirror:
                        //calcul de la nouvelle direction
                        Vector3 newDir     = Vector3.Add(Vector3.Multiply(2 * -Vector3.Dot(ray.direction, normal), normal), ray.direction);
                        Ray     reflection = new Ray(interPoint, newDir);
                        color = Vector3.Multiply(resInter.sph.material.albedo, Radiance(reflection, ++stop, arbre));
                        break;

                    case MaterialType.Light:
                        color = resInter.sph.material.albedo;
                        break;
                    }
                }
            }
            return(color);
        }
Esempio n. 35
0
    public override void Redraw( )
    {
        Clear();
        //GD.Print($"Redraw {GD.Randi()%100}");
        var     pathMaterial     = gizmoPlugin.GetMaterial("path_material", this);
        var     pathThinMaterial = gizmoPlugin.GetMaterial("path_thin_material", this);
        var     handlesMaterial  = gizmoPlugin.GetMaterial("handles", this);
        Curve3D c = path.curve;

        if (!IsInstanceValid(c))
        {
            GD.Print("Invalid curve");
            return;
        }

        var v3a = c.Tessellate();
        //PoolVector<Vector3> v3a=c.get_baked_points();

        int v3s = v3a.Length;

        if (v3s == 0)
        {
            return;
        }
        var v3p      = new Vector3[v3s * 2];
        int v3pCount = 0;

        // BUG: the following won't work when v3s, avoid drawing as a temporary workaround.
        for (int i = 0; i < v3s - 1; i++)
        {
            v3p = v3p.Add(ref v3pCount, v3a[i]);
            v3p = v3p.Add(ref v3pCount, v3a[i + 1]);
            //v3p.push_back(r[i]);
            //v3p.push_back(r[i]+Vector3(0,0.2,0));
        }

        if (v3pCount > 1)
        {
            v3p = v3p.Trim(ref v3pCount);
            AddLines((Vector3[])v3p.Clone(), pathMaterial);
            AddCollisionSegments((Vector3[])v3p.Clone());
        }

        if (plugin.path == path)
        {
            v3p = v3p.Clear(ref v3pCount, true);
            int pointCount      = c.GetPointCount();
            var handles         = new Vector3[pointCount];
            int handlesCount    = 0;
            var secHandles      = new Vector3[pointCount];
            int secHandlesCount = 0;

            for (int i = 0; i < pointCount; i++)
            {
                Vector3 p = c.GetPointPosition(i);
                handles = handles.Add(ref handlesCount, p);
                if (i > 0)
                {
                    v3p        = v3p.Add(ref v3pCount, p);
                    v3p        = v3p.Add(ref v3pCount, p + c.GetPointIn(i));
                    secHandles = secHandles.Add(ref secHandlesCount, p + c.GetPointIn(i));
                }

                if (i < pointCount - 1)
                {
                    v3p        = v3p.Add(ref v3pCount, p);
                    v3p        = v3p.Add(ref v3pCount, p + c.GetPointOut(i));
                    secHandles = secHandles.Add(ref secHandlesCount, p + c.GetPointOut(i));
                }
            }

            v3p        = v3p.Trim(ref v3pCount);
            handles    = handles.Trim(ref handlesCount);
            secHandles = secHandles.Trim(ref secHandlesCount);

            if (v3pCount > 1)
            {
                AddLines(v3p, pathThinMaterial);
            }
            if (handlesCount > 0)
            {
                AddHandles(handles, handlesMaterial);
            }
            if (secHandlesCount > 0)
            {
                AddHandles(secHandles, handlesMaterial, false, true);
            }
        }
    }
        public override void UpdateCollision(Fix64 dt)
        {
            WasContaining = Containing;
            WasTouching   = Touching;

            mobileTriangle.collisionMargin = mesh.Shape.MeshCollisionMargin;

            //Scan the pairs in sequence, updating the state as we go.
            //Touching can be set to true by a single touching subpair.
            Touching = false;
            //Containing can be set to false by a single noncontaining or nontouching subpair.
            Containing = true;


            var            meshData = mesh.Shape.TriangleMesh.Data;
            RigidTransform mobileTriangleTransform, detectorTriangleTransform;

            mobileTriangleTransform.Orientation   = Quaternion.Identity;
            detectorTriangleTransform.Orientation = Quaternion.Identity;
            for (int i = 0; i < meshData.Indices.Length; i += 3)
            {
                //Grab a triangle associated with the mobile mesh.
                meshData.GetTriangle(i, out mobileTriangle.vA, out mobileTriangle.vB, out mobileTriangle.vC);
                RigidTransform.Transform(ref mobileTriangle.vA, ref mesh.worldTransform, out mobileTriangle.vA);
                RigidTransform.Transform(ref mobileTriangle.vB, ref mesh.worldTransform, out mobileTriangle.vB);
                RigidTransform.Transform(ref mobileTriangle.vC, ref mesh.worldTransform, out mobileTriangle.vC);
                Vector3.Add(ref mobileTriangle.vA, ref mobileTriangle.vB, out mobileTriangleTransform.Position);
                Vector3.Add(ref mobileTriangle.vC, ref mobileTriangleTransform.Position, out mobileTriangleTransform.Position);
                Vector3.Multiply(ref mobileTriangleTransform.Position, F64.OneThird, out mobileTriangleTransform.Position);
                Vector3.Subtract(ref mobileTriangle.vA, ref mobileTriangleTransform.Position, out mobileTriangle.vA);
                Vector3.Subtract(ref mobileTriangle.vB, ref mobileTriangleTransform.Position, out mobileTriangle.vB);
                Vector3.Subtract(ref mobileTriangle.vC, ref mobileTriangleTransform.Position, out mobileTriangle.vC);

                //Go through all the detector volume triangles which are near the mobile mesh triangle.
                bool        triangleTouching, triangleContaining;
                BoundingBox mobileBoundingBox;
                mobileTriangle.GetBoundingBox(ref mobileTriangleTransform, out mobileBoundingBox);
                DetectorVolume.TriangleMesh.Tree.GetOverlaps(mobileBoundingBox, overlaps);
                for (int j = 0; j < overlaps.Count; j++)
                {
                    DetectorVolume.TriangleMesh.Data.GetTriangle(overlaps.Elements[j], out detectorTriangle.vA, out detectorTriangle.vB, out detectorTriangle.vC);
                    Vector3.Add(ref detectorTriangle.vA, ref detectorTriangle.vB, out detectorTriangleTransform.Position);
                    Vector3.Add(ref detectorTriangle.vC, ref detectorTriangleTransform.Position, out detectorTriangleTransform.Position);
                    Vector3.Multiply(ref detectorTriangleTransform.Position, F64.OneThird, out detectorTriangleTransform.Position);
                    Vector3.Subtract(ref detectorTriangle.vA, ref detectorTriangleTransform.Position, out detectorTriangle.vA);
                    Vector3.Subtract(ref detectorTriangle.vB, ref detectorTriangleTransform.Position, out detectorTriangle.vB);
                    Vector3.Subtract(ref detectorTriangle.vC, ref detectorTriangleTransform.Position, out detectorTriangle.vC);

                    //If this triangle collides with the convex, we can stop immediately since we know we're touching and not containing.)))
                    //[MPR is used here in lieu of GJK because the MPR implementation tends to finish quicker than GJK when objects are overlapping.  The GJK implementation does better on separated objects.]
                    if (MPRToolbox.AreShapesOverlapping(detectorTriangle, mobileTriangle, ref detectorTriangleTransform, ref mobileTriangleTransform))
                    {
                        triangleTouching = true;
                        //The convex can't be fully contained if it's still touching the surface.
                        triangleContaining = false;
                        overlaps.Clear();
                        goto finishTriangleTest;
                    }
                }

                overlaps.Clear();
                //If we get here, then there was no shell intersection.
                //If the convex's center point is contained by the mesh, then the convex is fully contained.
                //This test is only needed if containment hasn't yet been outlawed or a touching state hasn't been established.
                if ((!Touching || Containing) && DetectorVolume.IsPointContained(ref mobileTriangleTransform.Position, overlaps))
                {
                    triangleTouching   = true;
                    triangleContaining = true;
                    goto finishTriangleTest;
                }

                //If we get here, then there was no surface intersection and the convex's center is not contained- the volume and convex are separate!
                triangleTouching   = false;
                triangleContaining = false;

finishTriangleTest:
                //Analyze the results of the triangle test.

                if (triangleTouching)
                {
                    Touching = true; //If one child is touching, then we are touching too.
                }
                else
                {
                    Containing = false;  //If one child isn't touching, then we aren't containing.
                }
                if (!triangleContaining) //If one child isn't containing, then we aren't containing.
                {
                    Containing = false;
                }

                if (!Containing && Touching)
                {
                    //If it's touching but not containing, no further pairs will change the state.
                    //Containment has been invalidated by something that either didn't touch or wasn't contained.
                    //Touching has been ensured by at least one object touching.
                    break;
                }
            }

            //There is a possibility that the MobileMesh is solid and fully contains the DetectorVolume.
            //In this case, we should be Touching, but currently we are not.
            if (mesh.Shape.solidity == MobileMeshSolidity.Solid && !Containing && !Touching)
            {
                //To determine if the detector volume is fully contained, check if one of the detector mesh's vertices
                //are in the mobile mesh.

                //This *could* fail if the mobile mesh is actually multiple pieces, but that's not a common or really supported case for solids.
                Vector3 vertex;
                DetectorVolume.TriangleMesh.Data.GetVertexPosition(0, out vertex);
                Ray ray;
                ray.Direction = Vector3.Up;
                RayHit hit;
                RigidTransform.TransformByInverse(ref vertex, ref mesh.worldTransform, out ray.Position);
                if (mesh.Shape.IsLocalRayOriginInMesh(ref ray, out hit))
                {
                    Touching = true;
                }
            }

            NotifyDetectorVolumeOfChanges();
        }
Esempio n. 37
0
        public float DistanceSquareToSegment(Vector3 v0,Vector3 v1, ref Vector3 optionalPointOnRay, ref Vector3 optionalPointOnSegment)
        {
		// from http://www.geometrictools.com/LibMathematics/Distance/Wm5DistRay3Segment3.cpp
		// It returns the min distance between the ray and the segment
		// defined by v0 and v1
		// It can also set two optional targets :
		// - The closest point on the ray
		// - The closest point on the segment

		var segCenter = v0;
            segCenter.Add( v1 );
            segCenter.Multiply( 0.5f );

		var segDir = v1;
            segDir.Subtract(v0);
            segDir.Normalize();

		var segExtent = v0.DistanceTo( v1 ) / 2;
		var diff = origin;
            diff.Subtract(segCenter );

		var a01 = - direction.Dot( segDir );
		var b0 = diff.Dot( direction );
		var b1 = - diff.Dot( segDir );
		var c = diff.LengthSquared();
		var det = Mathf.Abs( 1 - a01 * a01 );
		var sqrDist = 0f;
            var s0 = 0f;
            var s1 = 0f;

            if ( det >= 0 ) {
			// The ray and segment are not parallel.
			s0 = a01 * b1 - b0;
			s1 = a01 * b0 - b1;
			var extDet = segExtent * det;

			if ( s0 >= 0 ) {

				if ( s1 >= - extDet ) {

					if ( s1 <= extDet ) {

						// region 0
						// Minimum at interior points of ray and segment.

						var invDet = 1 / det;
						s0 *= invDet;
						s1 *= invDet;
						sqrDist = s0 * ( s0 + a01 * s1 + 2 * b0 ) + s1 * ( a01 * s0 + s1 + 2 * b1 ) + c;

					} else {

						// region 1

						s1 = segExtent;
						s0 = Mathf.Max( 0, - ( a01 * s1 + b0 ) );
						sqrDist = - s0 * s0 + s1 * ( s1 + 2 * b1 ) + c;

					}

				} else {

					// region 5

					s1 = - segExtent;
					s0 = Mathf.Max( 0, - ( a01 * s1 + b0 ) );
					sqrDist = - s0 * s0 + s1 * ( s1 + 2 * b1 ) + c;

				}

			} else {

				if ( s1 <= - extDet ) {

					// region 4

					s0 = Mathf.Max( 0, - ( - a01 * segExtent + b0 ) );
					s1 = ( s0 > 0 ) ? - segExtent : Mathf.Min(Mathf.Max( - segExtent, - b1 ), segExtent );
					sqrDist = - s0 * s0 + s1 * ( s1 + 2 * b1 ) + c;

				} else if ( s1 <= extDet ) {

					// region 3

					s0 = 0;
					s1 = Mathf.Min(Mathf.Max( - segExtent, - b1 ), segExtent );
					sqrDist = s1 * ( s1 + 2 * b1 ) + c;

				} else {

					// region 2

					s0 = Mathf.Max( 0, - ( a01 * segExtent + b0 ) );
					s1 = ( s0 > 0 ) ? segExtent : Mathf.Min( Mathf.Max( - segExtent, - b1 ), segExtent );
					sqrDist = - s0 * s0 + s1 * ( s1 + 2 * b1 ) + c;

				}

			}

		} else {

			// Ray and segment are parallel.

			s1 = ( a01 > 0 ) ? - segExtent : segExtent;
			s0 = Mathf.Max( 0, - ( a01 * s1 + b0 ) );
			sqrDist = - s0 * s0 + s1 * ( s1 + 2 * b1 ) + c;

		}

		optionalPointOnRay = direction;
            optionalPointOnRay.Multiply(s0);
            optionalPointOnRay.Add(origin);

            optionalPointOnSegment = segDir;
            optionalPointOnSegment.Multiply( s1 );
            optionalPointOnSegment.Add( segCenter );

		return sqrDist;

        }
        private bool terrenoMuyEmpinado(Vector3 origin, Vector3 direction)
        {
            Vector3 deltaXZ = direction * 5;

            Vector3 target = new Vector3(origin.X, 0, origin.Z);
            target.Add(deltaXZ);
            float targetDeltaY = this.level.Terrain.getPosition(target.X, target.Z).Y - origin.Y;

            //if(targetDeltaY > MAX_DELTA_Y)GuiController.Instance.Logger.log("Pendiente: " + origin.Y + " -> " + (origin.Y + targetDeltaY) + " = " + targetDeltaY );

            return targetDeltaY > MAX_DELTA_Y;
        }