Beispiel #1
0
        public void UpdateWheelTransform(int wheelIndex, bool interpolatedTransform)
        {
            WheelInfo wheel = m_wheelInfo[wheelIndex];

            UpdateWheelTransformsWS(wheel, interpolatedTransform);
            IndexedVector3 up    = -wheel.m_raycastInfo.m_wheelDirectionWS;
            IndexedVector3 right = wheel.m_raycastInfo.m_wheelAxleWS;
            IndexedVector3 fwd   = IndexedVector3.Cross(up, right);

            fwd.Normalize();
            //	up = right.cross(fwd);
            //	up.normalize();

            //rotate around steering over de wheelAxleWS
            float steering = wheel.m_steering;

            IndexedQuaternion  steeringOrn = new IndexedQuaternion(up, steering);  //wheel.m_steering);
            IndexedBasisMatrix steeringMat = new IndexedBasisMatrix(ref steeringOrn);

            IndexedQuaternion  rotatingOrn = new IndexedQuaternion(right, -wheel.m_rotation);
            IndexedBasisMatrix rotatingMat = new IndexedBasisMatrix(ref rotatingOrn);


            IndexedBasisMatrix basis2 = new IndexedBasisMatrix(
                right.X, fwd.X, up.X,
                right.Y, fwd.Y, up.Y,
                right.Z, fwd.Z, up.Z
                );

            // FIXME MAN - MATRIX ORDER
            //wheel.m_worldTransform = steeringMat * rotatingMat * basis2;
            wheel.m_worldTransform._basis  = steeringMat * rotatingMat * basis2;
            wheel.m_worldTransform._origin =
                wheel.m_raycastInfo.m_hardPointWS + (wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength);
        }
Beispiel #2
0
        public void UpdateSeparatingDistance(ref IndexedMatrix transA, ref IndexedMatrix transB)
        {
            IndexedVector3 toPosA = transA._origin;
            IndexedVector3 toPosB = transB._origin;
            IndexedQuaternion toOrnA = transA.GetRotation();
            IndexedQuaternion toOrnB = transB.GetRotation();

            if (m_separatingDistance > 0.0f)
            {
                IndexedVector3 linVelA;
                IndexedVector3 angVelA;
                IndexedVector3 linVelB;
                IndexedVector3 angVelB;

                TransformUtil.CalculateVelocityQuaternion(ref m_posA, ref toPosA, ref m_ornA, ref toOrnA, 1f, out linVelA, out angVelA);
                TransformUtil.CalculateVelocityQuaternion(ref m_posB, ref toPosB, ref m_ornB, ref toOrnB, 1f, out linVelB, out angVelB);
                float maxAngularProjectedVelocity = angVelA.Length() * m_boundingRadiusA + angVelB.Length() * m_boundingRadiusB;
                IndexedVector3 relLinVel = (linVelB - linVelA);
                float relLinVelocLength = IndexedVector3.Dot((linVelB - linVelA), m_separatingNormal);
                if (relLinVelocLength < 0f)
                {
                    relLinVelocLength = 0f;
                }

                float projectedMotion = maxAngularProjectedVelocity + relLinVelocLength;
                m_separatingDistance -= projectedMotion;
            }

            m_posA = toPosA;
            m_posB = toPosB;
            m_ornA = toOrnA;
            m_ornB = toOrnB;
        }
Beispiel #3
0
        public static void CalculateDiffAxisAngle(ref IndexedMatrix transform0, ref IndexedMatrix transform1, out IndexedVector3 axis, out float angle)
        {
            //IndexedMatrix dmat = GetRotateMatrix(ref transform1) * IndexedMatrix.Invert(GetRotateMatrix(ref transform0));
            IndexedBasisMatrix dmat = transform1._basis * transform0._basis.Inverse();
            IndexedQuaternion dorn = IndexedQuaternion.Identity;
            GetRotation(ref dmat, out dorn);

            ///floating point inaccuracy can lead to w component > 1..., which breaks 
            dorn.Normalize();

            angle = MathUtil.QuatAngle(ref dorn);

            axis = new IndexedVector3(dorn.X, dorn.Y, dorn.Z);
            //axis[3] = float(0.);
            //check for axis length
            float len = axis.LengthSquared();
            if (len < MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
            {
                axis = new IndexedVector3(1,0,0);
            }
            else
            {
                axis.Normalize();
            }
        }
        public void SetAxis(ref IndexedVector3 axisInA)
        {
            IndexedVector3 rbAxisA1, rbAxisA2;

            TransformUtil.PlaneSpace1(ref axisInA, out rbAxisA1, out rbAxisA2);
            IndexedVector3 pivotInA = m_rbAFrame._origin;

            //		m_rbAFrame._origin = pivotInA;

            //MathUtil.setBasis(ref m_rbAFrame,ref axisInA,ref rbAxisA1,ref rbAxisA2);
            m_rbAFrame._basis = new IndexedBasisMatrix(rbAxisA1.X, rbAxisA2.X, axisInA.X,
                                                       rbAxisA1.Y, rbAxisA2.Y, axisInA.Y,
                                                       rbAxisA1.Z, rbAxisA2.Z, axisInA.Z);

            IndexedVector3 axisInB = m_rbA.GetCenterOfMassTransform()._basis *axisInA;

            IndexedQuaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB);
            IndexedVector3    rbAxisB1    = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1);
            IndexedVector3    rbAxisB2    = IndexedVector3.Cross(ref axisInB, ref rbAxisB1);

            m_rbBFrame._origin = m_rbB.GetCenterOfMassTransform().Inverse() * (m_rbA.GetCenterOfMassTransform() * (pivotInA));

            m_rbBFrame._basis = new IndexedBasisMatrix(rbAxisB1.X, rbAxisB2.X, axisInB.X,
                                                       rbAxisB1.Y, rbAxisB2.Y, axisInB.Y,
                                                       rbAxisB1.Z, rbAxisB2.Z, axisInB.Z);
        }
Beispiel #5
0
        public virtual void Rotate(IndexedQuaternion iq)
        {
            IndexedMatrix im = IndexedMatrix.CreateFromQuaternion(iq);

            im._origin = m_graphicsWorldTrans._origin;
            SetWorldTransform(ref im);
        }
        public static bool ClampNormal(ref IndexedVector3 edge, ref IndexedVector3 tri_normal_org, ref IndexedVector3 localContactNormalOnB, float correctedEdgeAngle, out IndexedVector3 clampedLocalNormal)
        {
            IndexedVector3 tri_normal = tri_normal_org;
            //we only have a local triangle normal, not a local contact normal . only normal in world space...
            //either compute the current angle all in local space, or all in world space

            IndexedVector3 edgeCross = IndexedVector3.Cross(edge, tri_normal).Normalized();
            float          curAngle  = GetAngle(ref edgeCross, ref tri_normal, ref localContactNormalOnB);

            if (correctedEdgeAngle < 0)
            {
                if (curAngle < correctedEdgeAngle)
                {
                    float             diffAngle = correctedEdgeAngle - curAngle;
                    IndexedQuaternion rotation  = new IndexedQuaternion(edge, diffAngle);
                    clampedLocalNormal = new IndexedBasisMatrix(rotation) * localContactNormalOnB;
                    return(true);
                }
            }

            if (correctedEdgeAngle >= 0)
            {
                if (curAngle > correctedEdgeAngle)
                {
                    float             diffAngle = correctedEdgeAngle - curAngle;
                    IndexedQuaternion rotation  = new IndexedQuaternion(edge, diffAngle);
                    clampedLocalNormal = new IndexedBasisMatrix(rotation) * localContactNormalOnB;
                    return(true);
                }
            }
            clampedLocalNormal = IndexedVector3.Zero;
            return(false);
        }
        public void SetMotorTarget(ref IndexedQuaternion qAinB, float dt)         // qAinB is rotation of body A wrt body B.
        {
            // convert target from body to constraint space
            IndexedQuaternion qConstraint = MathUtil.QuaternionInverse(m_rbBFrame.GetRotation()) * qAinB * m_rbAFrame.GetRotation();

            qConstraint.Normalize();

            // extract "pure" hinge component
            IndexedVector3 vNoHinge = MathUtil.QuatRotate(ref qConstraint, ref vHinge);

            vNoHinge.Normalize();
            IndexedQuaternion qNoHinge = MathUtil.ShortestArcQuat(ref vHinge, ref vNoHinge);
            IndexedQuaternion qHinge   = MathUtil.QuaternionInverse(ref qNoHinge) * qConstraint;

            qHinge.Normalize();

            // compute angular target, clamped to limits
            float targetAngle = MathUtil.QuatAngle(ref qHinge);

            if (targetAngle > MathUtil.SIMD_PI)             // long way around. flip quat and recalculate.
            {
                qHinge      = -qHinge;
                targetAngle = MathUtil.QuatAngle(ref qHinge);
            }
            if (qHinge.Z < 0)
            {
                targetAngle = -targetAngle;
            }

            SetMotorTarget(targetAngle, dt);
        }
Beispiel #8
0
 internal static bool AlmostEqual(ref IndexedQuaternion v1, ref IndexedQuaternion v2, float nEpsilon)
 {
     return
         ((((v1.X - nEpsilon) < v2.X) && (v2.X < (v1.X + nEpsilon))) &&
          (((v1.Y - nEpsilon) < v2.Y) && (v2.Y < (v1.Y + nEpsilon))) &&
          (((v1.Z - nEpsilon) < v2.Z) && (v2.Z < (v1.Z + nEpsilon))) &&
          (((v1.W - nEpsilon) < v2.W) && (v2.W < (v1.W + nEpsilon))));
 }
Beispiel #9
0
        public EntityProperties FromTransform(uint id, IndexedMatrix startTransform)
        {
            EntityProperties ret = new EntityProperties();

            ID       = id;
            Position = startTransform._origin;
            Rotation = startTransform.GetRotation();
            return(ret);
        }
Beispiel #10
0
        void InitSeparatingDistance(ref IndexedVector3 separatingVector, float separatingDistance, ref IndexedMatrix transA, ref IndexedMatrix transB)
	    {
		    m_separatingNormal = separatingVector;
		    m_separatingDistance = separatingDistance;
    		
		    IndexedVector3 toPosA = transA._origin;
		    IndexedVector3 toPosB = transB._origin;
            IndexedQuaternion toOrnA = transA.GetRotation();
            IndexedQuaternion toOrnB = transB.GetRotation();
		    m_posA = toPosA;
		    m_posB = toPosB;
		    m_ornA = toOrnA;
		    m_ornB = toOrnB;
	    }
Beispiel #11
0
	    public static void IntegrateTransform(ref IndexedMatrix curTrans,ref IndexedVector3 linvel,ref IndexedVector3 angvel,float timeStep,out IndexedMatrix predictedTransform)
	    {
            predictedTransform = IndexedMatrix.CreateTranslation(curTrans._origin + linvel * timeStep);
    //	#define QUATERNION_DERIVATIVE
	    #if QUATERNION_DERIVATIVE
            IndexedVector3 pos;
            IndexedQuaternion predictedOrn;
            IndexedVector3 scale;

            curTrans.Decompose(ref scale, ref predictedOrn, ref pos);


		    predictedOrn += (angvel * predictedOrn) * (timeStep * .5f));
		    predictedOrn.Normalize();
        #else
            //Exponential map
		    //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia

		    IndexedVector3 axis;
		    float	fAngle = angvel.Length(); 
		    //limit the angular motion
		    if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
		    {
			    fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
		    }

		    if ( fAngle < 0.001f )
		    {
			    // use Taylor's expansions of sync function
			    axis   = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle );
		    }
		    else
		    {
			    // sync(fAngle) = sin(c*fAngle)/t
			    axis   = angvel*( (float)Math.Sin(0.5f*fAngle*timeStep)/fAngle );
		    }
		    IndexedQuaternion dorn = new IndexedQuaternion(axis.X,axis.Y,axis.Z,(float)Math.Cos( fAngle*timeStep*.5f) );

            IndexedQuaternion orn0 = curTrans.GetRotation();

		    IndexedQuaternion predictedOrn = dorn * orn0;
		    predictedOrn.Normalize();
	    #endif

            IndexedMatrix newMatrix = IndexedMatrix.CreateFromQuaternion(predictedOrn);
            predictedTransform._basis = newMatrix._basis;
	    }
        public HingeConstraint(RigidBody rbA, ref IndexedVector3 pivotInA, ref IndexedVector3 axisInA, bool useReferenceFrameA)
            : base(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA)
        {
            m_angularOnly                 = false;
            m_enableAngularMotor          = false;
            m_useReferenceFrameA          = useReferenceFrameA;
            m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET;
            m_flags = 0;

#if     _BT_USE_CENTER_LIMIT_
            m_limit = new AngularLimit();
#endif

            // since no frame is given, assume this to be zero angle and just pick rb transform axis
            // fixed axis in worldspace
            IndexedVector3 rbAxisA1, rbAxisA2;
            TransformUtil.PlaneSpace1(ref axisInA, out rbAxisA1, out rbAxisA2);

            m_rbAFrame._origin = pivotInA;
            //m_rbAFrame._basis = new IndexedBasisMatrix(ref rbAxisA1, ref rbAxisA2, ref axisInA);
            m_rbAFrame._basis = new IndexedBasisMatrix(rbAxisA1.X, rbAxisA2.X, axisInA.X,
                                                       rbAxisA1.Y, rbAxisA2.Y, axisInA.Y,
                                                       rbAxisA1.Z, rbAxisA2.Z, axisInA.Z);

            IndexedVector3 axisInB = rbA.GetCenterOfMassTransform()._basis *axisInA;

            IndexedQuaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB);
            IndexedVector3    rbAxisB1    = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1);
            IndexedVector3    rbAxisB2    = IndexedVector3.Cross(axisInB, rbAxisB1);

            m_rbBFrame._origin = rbA.GetCenterOfMassTransform() * pivotInA;
            //m_rbBFrame._basis = new IndexedBasisMatrix(ref rbAxisB1, ref rbAxisB2, ref axisInB);
            m_rbBFrame._basis = new IndexedBasisMatrix(rbAxisB1.X, rbAxisB2.X, axisInB.X,
                                                       rbAxisB1.Y, rbAxisB2.Y, axisInB.Y,
                                                       rbAxisB1.Z, rbAxisB2.Z, axisInB.Z);

            //start with free
#if !_BT_USE_CENTER_LIMIT_
            m_lowerLimit       = 1f;
            m_upperLimit       = -1f;
            m_biasFactor       = 0.3f;
            m_relaxationFactor = 1.0f;
            m_limitSoftness    = 0.9f;
            m_solveLimit       = false;
#endif
            m_referenceSign = m_useReferenceFrameA ? -1.0f : 1.0f;
        }
Beispiel #13
0
        public static void CalculateDiffAxisAngleQuaternion(ref IndexedQuaternion orn0, ref IndexedQuaternion orn1a, out IndexedVector3 axis, out float angle)
        {
            IndexedQuaternion orn1 = MathUtil.QuatFurthest(ref orn0, ref orn1a);
            IndexedQuaternion dorn = orn1 * MathUtil.QuaternionInverse(ref orn0);

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

            //check for axis length
            float len = axis.LengthSquared();
            if (len < MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
            {
                axis = new IndexedVector3(1f, 0, 0);
            }
            else
            {
                axis.Normalize();
            }
        }
        public virtual void CollideSingleContact(ref IndexedQuaternion perturbeRot, CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            CollisionObject convexObj = m_isSwapped ? body1 : body0;
            CollisionObject planeObj  = m_isSwapped ? body0 : body1;

            ConvexShape      convexShape = convexObj.GetCollisionShape() as ConvexShape;
            StaticPlaneShape planeShape  = planeObj.GetCollisionShape() as StaticPlaneShape;

            bool           hasCollision  = false;
            IndexedVector3 planeNormal   = planeShape.GetPlaneNormal();
            float          planeConstant = planeShape.GetPlaneConstant();

            IndexedMatrix convexWorldTransform = convexObj.GetWorldTransform();
            IndexedMatrix convexInPlaneTrans   = planeObj.GetWorldTransform().Inverse() * convexWorldTransform;

            //now perturbe the convex-world transform

            convexWorldTransform._basis *= new IndexedBasisMatrix(ref perturbeRot);

            IndexedMatrix planeInConvex = convexWorldTransform.Inverse() * planeObj.GetWorldTransform();;

            IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);

            IndexedVector3 vtxInPlane = vtxInPlane = convexInPlaneTrans * vtx;
            float          distance   = (IndexedVector3.Dot(planeNormal, vtxInPlane) - planeConstant);

            IndexedVector3 vtxInPlaneProjected = vtxInPlane - (distance * planeNormal);
            IndexedVector3 vtxInPlaneWorld     = planeObj.GetWorldTransform() * vtxInPlaneProjected;

            hasCollision = distance < m_manifoldPtr.GetContactBreakingThreshold();

            resultOut.SetPersistentManifold(m_manifoldPtr);
            if (hasCollision)
            {
                /// report a contact. internally this will be kept persistent, and contact reduction is done
                IndexedVector3 normalOnSurfaceB = planeObj.GetWorldTransform()._basis *planeNormal;
                IndexedVector3 pOnB             = vtxInPlaneWorld;
                resultOut.AddContactPoint(ref normalOnSurfaceB, ref pOnB, distance);
            }
        }
            public virtual void ProcessTriangle(IndexedVector3[] triangle, int partId, int triangleIndex)
            {
                //skip self-collisions
                if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))
                {
                    return;
                }

                //skip duplicates (disabled for now)
                //if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex))
                //	return;

                //search for shared vertices and edges
                int numshared = 0;

                int[] sharedVertsA = new int[] { -1, -1, -1 };
                int[] sharedVertsB = new int[] { -1, -1, -1 };

                ///skip degenerate triangles
                float crossBSqr = IndexedVector3.Cross((triangle[1] - triangle[0]), (triangle[2] - triangle[0])).LengthSquared();

                if (crossBSqr < m_triangleInfoMap.m_equalVertexThreshold)
                {
                    return;
                }

                float crossASqr = IndexedVector3.Cross((m_triangleVerticesA[1] - m_triangleVerticesA[0]), (m_triangleVerticesA[2] - m_triangleVerticesA[0])).LengthSquared();

                ///skip degenerate triangles
                if (crossASqr < m_triangleInfoMap.m_equalVertexThreshold)
                {
                    return;
                }

#if false
                printf("triangle A[0]	=	(%f,%f,%f)\ntriangle A[1]	=	(%f,%f,%f)\ntriangle A[2]	=	(%f,%f,%f)\n",
                       m_triangleVerticesA[0].GetX(), m_triangleVerticesA[0].GetY(), m_triangleVerticesA[0].GetZ(),
                       m_triangleVerticesA[1].GetX(), m_triangleVerticesA[1].GetY(), m_triangleVerticesA[1].GetZ(),
                       m_triangleVerticesA[2].GetX(), m_triangleVerticesA[2].GetY(), m_triangleVerticesA[2].GetZ());

                printf("partId=%d, triangleIndex=%d\n", partId, triangleIndex);
                printf("triangle B[0]	=	(%f,%f,%f)\ntriangle B[1]	=	(%f,%f,%f)\ntriangle B[2]	=	(%f,%f,%f)\n",
                       triangle[0].GetX(), triangle[0].GetY(), triangle[0].GetZ(),
                       triangle[1].GetX(), triangle[1].GetY(), triangle[1].GetZ(),
                       triangle[2].GetX(), triangle[2].GetY(), triangle[2].GetZ());
#endif

                for (int i = 0; i < 3; i++)
                {
                    for (int j = 0; j < 3; j++)
                    {
                        if ((m_triangleVerticesA[i] - triangle[j]).LengthSquared() < m_triangleInfoMap.m_equalVertexThreshold)
                        {
                            sharedVertsA[numshared] = i;
                            sharedVertsB[numshared] = j;
                            numshared++;
                            ///degenerate case
                            if (numshared >= 3)
                            {
                                return;
                            }
                        }
                    }
                    ///degenerate case
                    if (numshared >= 3)
                    {
                        return;
                    }
                }
                switch (numshared)
                {
                case 0:
                {
                    break;
                }

                case 1:
                {
                    //shared vertex
                    break;
                }

                case 2:
                {
                    //shared edge
                    //we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct
                    if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
                    {
                        sharedVertsA[0] = 2;
                        sharedVertsA[1] = 0;
                        int tmp = sharedVertsB[1];
                        sharedVertsB[1] = sharedVertsB[0];
                        sharedVertsB[0] = tmp;
                    }

                    int hash = GetHash(m_partIdA, m_triangleIndexA);


                    TriangleInfo info = null;
                    if (m_triangleInfoMap.ContainsKey(hash))
                    {
                        info = m_triangleInfoMap[hash];
                    }
                    else
                    {
                        info = new TriangleInfo();
                        m_triangleInfoMap[hash] = info;
                    }

                    int sumvertsA   = sharedVertsA[0] + sharedVertsA[1];
                    int otherIndexA = 3 - sumvertsA;


                    IndexedVector3 edge = new IndexedVector3(m_triangleVerticesA[sharedVertsA[1]] - m_triangleVerticesA[sharedVertsA[0]]);

                    TriangleShape tA          = new TriangleShape(m_triangleVerticesA[0], m_triangleVerticesA[1], m_triangleVerticesA[2]);
                    int           otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]);

                    TriangleShape tB = new TriangleShape(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]);
                    //btTriangleShape tB(triangle[0],triangle[1],triangle[2]);

                    IndexedVector3 normalA;
                    IndexedVector3 normalB;
                    tA.CalcNormal(out normalA);
                    tB.CalcNormal(out normalB);
                    edge.Normalize();
                    IndexedVector3 edgeCrossA = IndexedVector3.Normalize(IndexedVector3.Cross(edge, normalA));

                    {
                        IndexedVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]];
                        if (IndexedVector3.Dot(edgeCrossA, tmp) < 0)
                        {
                            edgeCrossA *= -1;
                        }
                    }

                    IndexedVector3 edgeCrossB = IndexedVector3.Cross(edge, normalB).Normalized();

                    {
                        IndexedVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]];
                        if (IndexedVector3.Dot(edgeCrossB, tmp) < 0)
                        {
                            edgeCrossB *= -1;
                        }
                    }

                    float angle2 = 0;
                    float ang4   = 0.0f;

                    IndexedVector3 calculatedEdge = IndexedVector3.Cross(edgeCrossA, edgeCrossB);
                    float          len2           = calculatedEdge.LengthSquared();

                    float          correctedAngle    = 0f;
                    IndexedVector3 calculatedNormalB = normalA;
                    bool           isConvex          = false;

                    if (len2 < m_triangleInfoMap.m_planarEpsilon)
                    {
                        angle2 = 0.0f;
                        ang4   = 0.0f;
                    }
                    else
                    {
                        calculatedEdge.Normalize();
                        IndexedVector3 calculatedNormalA = IndexedVector3.Cross(calculatedEdge, edgeCrossA);
                        calculatedNormalA.Normalize();
                        angle2 = GetAngle(ref calculatedNormalA, ref edgeCrossA, ref edgeCrossB);
                        ang4   = MathUtil.SIMD_PI - angle2;
                        float dotA = IndexedVector3.Dot(normalA, edgeCrossB);
                        ///@todo: check if we need some epsilon, due to floating point imprecision
                        isConvex = (dotA < 0f);

                        correctedAngle = isConvex ? ang4 : -ang4;
                        IndexedQuaternion orn2         = new IndexedQuaternion(calculatedEdge, -correctedAngle);
                        IndexedMatrix     rotateMatrix = IndexedMatrix.CreateFromQuaternion(orn2);
                        calculatedNormalB = new IndexedBasisMatrix(orn2) * normalA;
                    }


                    //alternatively use
                    //IndexedVector3 calculatedNormalB2 = quatRotate(orn,normalA);


                    switch (sumvertsA)
                    {
                    case 1:
                    {
                        IndexedVector3    edge1           = m_triangleVerticesA[0] - m_triangleVerticesA[1];
                        IndexedQuaternion orn             = new IndexedQuaternion(edge1, -correctedAngle);
                        IndexedVector3    computedNormalB = MathUtil.QuatRotate(orn, normalA);
                        float             bla             = IndexedVector3.Dot(computedNormalB, normalB);
                        if (bla < 0)
                        {
                            computedNormalB *= -1;
                            info.m_flags    |= TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB;
                        }
#if DEBUG_INTERNAL_EDGE
                        if ((computedNormalB - normalB).Length() > 0.0001f)
                        {
                            System.Console.WriteLine("warning: normals not identical");
                        }
#endif//DEBUG_INTERNAL_EDGE

                        info.m_edgeV0V1Angle = -correctedAngle;

                        if (isConvex)
                        {
                            info.m_flags |= TriangleInfoMap.TRI_INFO_V0V1_CONVEX;
                        }
                        break;
                    }

                    case 2:
                    {
                        IndexedVector3    edge1           = m_triangleVerticesA[2] - m_triangleVerticesA[0];
                        IndexedQuaternion orn             = new IndexedQuaternion(edge1, -correctedAngle);
                        IndexedVector3    computedNormalB = MathUtil.QuatRotate(orn, normalA);
                        if (IndexedVector3.Dot(computedNormalB, normalB) < 0)
                        {
                            computedNormalB *= -1;
                            info.m_flags    |= TriangleInfoMap.TRI_INFO_V2V0_SWAP_NORMALB;
                        }

#if DEBUG_INTERNAL_EDGE
                        if ((computedNormalB - normalB).Length() > 0.0001)
                        {
                            System.Console.WriteLine("warning: normals not identical");
                        }
#endif //DEBUG_INTERNAL_EDGE
                        info.m_edgeV2V0Angle = -correctedAngle;
                        if (isConvex)
                        {
                            info.m_flags |= TriangleInfoMap.TRI_INFO_V2V0_CONVEX;
                        }
                        break;
                    }

                    case 3:
                    {
                        IndexedVector3    edge1           = m_triangleVerticesA[1] - m_triangleVerticesA[2];
                        IndexedQuaternion orn             = new IndexedQuaternion(edge1, -correctedAngle);
                        IndexedVector3    computedNormalB = MathUtil.QuatRotate(orn, normalA);
                        if (IndexedVector3.Dot(computedNormalB, normalB) < 0)
                        {
                            info.m_flags    |= TriangleInfoMap.TRI_INFO_V1V2_SWAP_NORMALB;
                            computedNormalB *= -1;
                        }
#if DEBUG_INTERNAL_EDGE
                        if ((computedNormalB - normalB).Length() > 0.0001)
                        {
                            System.Console.WriteLine("warning: normals not identical");
                        }
#endif //DEBUG_INTERNAL_EDGE
                        info.m_edgeV1V2Angle = -correctedAngle;

                        if (isConvex)
                        {
                            info.m_flags |= TriangleInfoMap.TRI_INFO_V1V2_CONVEX;
                        }
                        break;
                    }
                    }

                    break;
                }

                default:
                {
                    //				printf("warning: duplicate triangle\n");
                    break;
                }
                }
            }
        /// Changes a btManifoldPoint collision normal to the normal from the mesh.
        public static void AdjustInternalEdgeContacts(ManifoldPoint cp, CollisionObject colObj0, CollisionObject colObj1, int partId0, int index0, InternalEdgeAdjustFlags normalAdjustFlags)
        {
            //btAssert(colObj0.GetCollisionShape().GetShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
            if (colObj0.GetCollisionShape().GetShapeType() != BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
            {
                return;
            }

            BvhTriangleMeshShape trimesh = null;

            if (colObj0.GetRootCollisionShape().GetShapeType() == BroadphaseNativeTypes.SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
            {
                //trimesh = ((ScaledBvhTriangleMeshShape)colObj0.GetRootCollisionShape()).GetChildShape();
            }
            else
            {
                trimesh = (BvhTriangleMeshShape)colObj0.GetRootCollisionShape();
            }

            TriangleInfoMap triangleInfoMapPtr = (TriangleInfoMap)trimesh.GetTriangleInfoMap();

            if (triangleInfoMapPtr == null)
            {
                return;
            }

            int hash = GetHash(partId0, index0);

            TriangleInfo info;

            if (!triangleInfoMapPtr.TryGetValue(hash, out info))
            {
                return;
            }


            float frontFacing = (normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_BACKFACE_MODE) == 0 ? 1.0f : -1.0f;

            TriangleShape  tri_shape = colObj0.GetCollisionShape() as TriangleShape;
            IndexedVector3 v0, v1, v2;

            tri_shape.GetVertex(0, out v0);
            tri_shape.GetVertex(1, out v1);
            tri_shape.GetVertex(2, out v2);

            IndexedVector3 center = (v0 + v1 + v2) * (1.0f / 3.0f);

            IndexedVector3 red = new IndexedVector3(1, 0, 0), green = new IndexedVector3(0, 1, 0), blue = new IndexedVector3(0, 0, 1), white = new IndexedVector3(1, 1, 1), black = new IndexedVector3(0, 0, 0);
            IndexedVector3 tri_normal;

            tri_shape.CalcNormal(out tri_normal);

            //float dot = tri_normal.dot(cp.m_normalWorldOnB);
            IndexedVector3 nearest;

            NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest);

            IndexedVector3 contact = cp.m_localPointB;

#if BT_INTERNAL_EDGE_DEBUG_DRAW
            IndexedMatrix tr = colObj0.GetWorldTransform();
            DebugDrawLine(tr * nearest, tr * cp.m_localPointB, red);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW



            bool isNearEdge = false;

            int numConcaveEdgeHits = 0;
            int numConvexEdgeHits  = 0;

            IndexedVector3 localContactNormalOnB = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
            localContactNormalOnB.Normalize();//is this necessary?

            // Get closest edge
            int   bestedge       = -1;
            float disttobestedge = MathUtil.BT_LARGE_FLOAT;
            //
            // Edge 0 . 1
            if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
                //IndexedVector3 nearest;
                NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest);
                float len = (contact - nearest).Length();
                //
                if (len < disttobestedge)
                {
                    bestedge       = 0;
                    disttobestedge = len;
                }
            }
            // Edge 1 . 2
            if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
                //IndexedVector3 nearest;
                NearestPointInLineSegment(ref cp.m_localPointB, ref v1, ref v2, out nearest);
                float len = (contact - nearest).Length();
                //
                if (len < disttobestedge)
                {
                    bestedge       = 1;
                    disttobestedge = len;
                }
            }
            // Edge 2 . 0
            if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
                //IndexedVector3 nearest;
                NearestPointInLineSegment(ref cp.m_localPointB, ref v2, ref v0, out nearest);
                float len = (contact - nearest).Length();
                //
                if (len < disttobestedge)
                {
                    bestedge       = 2;
                    disttobestedge = len;
                }
            }

#if BT_INTERNAL_EDGE_DEBUG_DRAW
            IndexedVector3 upfix = tri_normal * new IndexedVector3(0.1f, 0.1f, 0.1f);
            DebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red);
#endif
            if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif
                float len = (contact - nearest).Length();
                if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
                {
                    if (bestedge == 0)
                    {
                        IndexedVector3 edge = (v0 - v1);
                        isNearEdge = true;

                        if (info.m_edgeV0V1Angle == 0.0f)
                        {
                            numConcaveEdgeHits++;
                        }
                        else
                        {
                            bool  isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_CONVEX) != 0;
                            float swapFactor   = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                            DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                            IndexedVector3 nA = swapFactor * tri_normal;

                            IndexedQuaternion orn             = new IndexedQuaternion(edge, info.m_edgeV0V1Angle);
                            IndexedVector3    computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
                            if ((info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB) != 0)
                            {
                                computedNormalB *= -1;
                            }
                            IndexedVector3 nB = swapFactor * computedNormalB;

                            float NdotA            = localContactNormalOnB.Dot(ref nA);
                            float NdotB            = localContactNormalOnB.Dot(ref nB);
                            bool  backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);

#if DEBUG_INTERNAL_EDGE
                            {
                                DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
                            }
#endif //DEBUG_INTERNAL_EDGE


                            if (backFacingNormal)
                            {
                                numConcaveEdgeHits++;
                            }
                            else
                            {
                                numConvexEdgeHits++;
                                IndexedVector3 clampedLocalNormal;
                                bool           isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info.m_edgeV0V1Angle, out clampedLocalNormal);
                                if (isClamped)
                                {
                                    if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
                                    {
                                        IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis *clampedLocalNormal;
                                        //					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
                                        cp.m_normalWorldOnB = newNormal;
                                        // Reproject collision point along normal. (what about cp.m_distance1?)
                                        cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
                                        cp.m_localPointB      = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
                                    }
                                }
                            }
                        }
                    }
                }
            }

            NearestPointInLineSegment(ref contact, ref v1, ref v2, out nearest);
#if BT_INTERNAL_EDGE_DEBUG_DRAW
            DebugDrawLine(tr * nearest, tr * cp.m_localPointB, green);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

#if BT_INTERNAL_EDGE_DEBUG_DRAW
            DebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green);
#endif

            if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW



                float len = (contact - nearest).Length();
                if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
                {
                    if (bestedge == 1)
                    {
                        isNearEdge = true;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                        DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                        IndexedVector3 edge = (v1 - v2);

                        isNearEdge = true;

                        if (info.m_edgeV1V2Angle == 0f)
                        {
                            numConcaveEdgeHits++;
                        }
                        else
                        {
                            bool  isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_CONVEX) != 0;
                            float swapFactor   = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                            DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                            IndexedVector3 nA = swapFactor * tri_normal;

                            IndexedQuaternion orn             = new IndexedQuaternion(edge, info.m_edgeV1V2Angle);
                            IndexedVector3    computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
                            if ((info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_SWAP_NORMALB) != 0)
                            {
                                computedNormalB *= -1;
                            }
                            IndexedVector3 nB = swapFactor * computedNormalB;

#if DEBUG_INTERNAL_EDGE
                            {
                                DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
                            }
#endif //DEBUG_INTERNAL_EDGE


                            float NdotA            = localContactNormalOnB.Dot(ref nA);
                            float NdotB            = localContactNormalOnB.Dot(ref nB);
                            bool  backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);

                            if (backFacingNormal)
                            {
                                numConcaveEdgeHits++;
                            }
                            else
                            {
                                numConvexEdgeHits++;
                                IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
                                IndexedVector3 clampedLocalNormal;
                                bool           isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV1V2Angle, out clampedLocalNormal);
                                if (isClamped)
                                {
                                    if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
                                    {
                                        IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis *clampedLocalNormal;
                                        //					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
                                        cp.m_normalWorldOnB = newNormal;
                                        // Reproject collision point along normal.
                                        cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
                                        cp.m_localPointB      = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
                                    }
                                }
                            }
                        }
                    }
                }
            }

            NearestPointInLineSegment(ref contact, ref v2, ref v0, out nearest);
#if BT_INTERNAL_EDGE_DEBUG_DRAW
            DebugDrawLine(tr * nearest, tr * cp.m_localPointB, blue);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#if BT_INTERNAL_EDGE_DEBUG_DRAW
            DebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue);
#endif

            if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
            {
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                float len = (contact - nearest).Length();
                if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
                {
                    if (bestedge == 2)
                    {
                        isNearEdge = true;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                        DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                        IndexedVector3 edge = (v2 - v0);

                        if (info.m_edgeV2V0Angle == 0f)
                        {
                            numConcaveEdgeHits++;
                        }
                        else
                        {
                            bool  isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_CONVEX) != 0;
                            float swapFactor   = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
                            DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

                            IndexedVector3    nA              = swapFactor * tri_normal;
                            IndexedQuaternion orn             = new IndexedQuaternion(edge, info.m_edgeV2V0Angle);
                            IndexedVector3    computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
                            if ((info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_SWAP_NORMALB) != 0)
                            {
                                computedNormalB *= -1;
                            }
                            IndexedVector3 nB = swapFactor * computedNormalB;

#if DEBUG_INTERNAL_EDGE
                            {
                                DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
                            }
#endif //DEBUG_INTERNAL_EDGE

                            float NdotA            = localContactNormalOnB.Dot(ref nA);
                            float NdotB            = localContactNormalOnB.Dot(ref nB);
                            bool  backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);

                            if (backFacingNormal)
                            {
                                numConcaveEdgeHits++;
                            }
                            else
                            {
                                numConvexEdgeHits++;
                                //				printf("hitting convex edge\n");


                                IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
                                IndexedVector3 clampedLocalNormal;
                                bool           isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV2V0Angle, out clampedLocalNormal);
                                if (isClamped)
                                {
                                    if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
                                    {
                                        IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis *clampedLocalNormal;
                                        //					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
                                        cp.m_normalWorldOnB = newNormal;
                                        // Reproject collision point along normal.
                                        cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
                                        cp.m_localPointB      = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
                                    }
                                }
                            }
                        }
                    }
                }
            }

#if DEBUG_INTERNAL_EDGE
            {
                IndexedVector3 color = new IndexedVector3(0, 1, 1);
                DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + cp.m_normalWorldOnB * 10, color);
            }
#endif //DEBUG_INTERNAL_EDGE

            if (isNearEdge)
            {
                if (numConcaveEdgeHits > 0)
                {
                    if ((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONCAVE_DOUBLE_SIDED) != 0)
                    {
                        //fix tri_normal so it pointing the same direction as the current local contact normal
                        if (tri_normal.Dot(ref localContactNormalOnB) < 0)
                        {
                            tri_normal *= -1;
                        }
                        cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis *tri_normal;
                    }
                    else
                    {
                        IndexedVector3 newNormal = tri_normal * frontFacing;
                        //if the tri_normal is pointing opposite direction as the current local contact normal, skip it
                        float d = newNormal.Dot(ref localContactNormalOnB);
                        if (d < 0)
                        {
                            return;
                        }
                        //modify the normal to be the triangle normal (or backfacing normal)
                        cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis *newNormal;
                    }

                    // Reproject collision point along normal.
                    cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
                    cp.m_localPointB      = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
                }
            }
        }
Beispiel #17
0
 public static void CalculateVelocityQuaternion(ref IndexedVector3 pos0, ref IndexedVector3 pos1, ref IndexedQuaternion orn0, ref IndexedQuaternion orn1, float timeStep, out IndexedVector3 linVel, out IndexedVector3 angVel)
 {
     linVel = (pos1 - pos0) / timeStep;
     if (orn0 != orn1)
     {
         IndexedVector3 axis;
         float angle;
         CalculateDiffAxisAngleQuaternion(ref orn0, ref orn1, out axis, out angle);
         angVel = axis * (angle / timeStep);
     }
     else
     {
         angVel = IndexedVector3.Zero;
     }
 }
        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                return;
            }

            CollisionObject convexObj = m_isSwapped ? body1 : body0;
            CollisionObject planeObj  = m_isSwapped ? body0 : body1;

            ConvexShape      convexShape = convexObj.GetCollisionShape() as ConvexShape;
            StaticPlaneShape planeShape  = planeObj.GetCollisionShape() as StaticPlaneShape;

            bool           hasCollision  = false;
            IndexedVector3 planeNormal   = planeShape.GetPlaneNormal();
            float          planeConstant = planeShape.GetPlaneConstant();
            IndexedMatrix  planeInConvex;

            planeInConvex = convexObj.GetWorldTransform().Inverse() * planeObj.GetWorldTransform();
            IndexedMatrix convexInPlaneTrans;

            convexInPlaneTrans = planeObj.GetWorldTransform().Inverse() * convexObj.GetWorldTransform();

            IndexedVector3 vtx        = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);
            IndexedVector3 vtxInPlane = convexInPlaneTrans * vtx;
            float          distance   = (planeNormal.Dot(vtxInPlane) - planeConstant);

            IndexedVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
            IndexedVector3 vtxInPlaneWorld     = planeObj.GetWorldTransform() * vtxInPlaneProjected;

            hasCollision = distance < m_manifoldPtr.GetContactBreakingThreshold();
            resultOut.SetPersistentManifold(m_manifoldPtr);
            if (hasCollision)
            {
                /// report a contact. internally this will be kept persistent, and contact reduction is done
                IndexedVector3 normalOnSurfaceB = planeObj.GetWorldTransform()._basis *planeNormal;
                IndexedVector3 pOnB             = vtxInPlaneWorld;
                resultOut.AddContactPoint(normalOnSurfaceB, pOnB, distance);
            }
            ////first perform a collision query with the non-perturbated collision objects
            //{
            //    IndexedQuaternion rotq = IndexedQuaternion.Identity;
            //    CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut);
            //}

            if (convexShape.IsPolyhedral() && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
            {
                IndexedVector3 v0;
                IndexedVector3 v1;
                TransformUtil.PlaneSpace1(ref planeNormal, out v0, out v1);
                //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects

                float angleLimit = 0.125f * MathUtil.SIMD_PI;
                float perturbeAngle;
                float radius = convexShape.GetAngularMotionDisc();
                perturbeAngle = BulletGlobals.gContactBreakingThreshold / radius;
                if (perturbeAngle > angleLimit)
                {
                    perturbeAngle = angleLimit;
                }
                IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle);
                for (int i = 0; i < m_numPerturbationIterations; i++)
                {
                    float             iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
                    IndexedQuaternion rotq           = new IndexedQuaternion(planeNormal, iterationAngle);
                    rotq = IndexedQuaternion.Inverse(rotq) * perturbeRot * rotq;
                    CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut);
                }
            }

            if (m_ownManifold)
            {
                if (m_manifoldPtr.GetNumContacts() > 0)
                {
                    resultOut.RefreshContactPoints();
                }
            }
        }
Beispiel #19
0
 public static void GetRotation(ref IndexedBasisMatrix a, out IndexedQuaternion rot)
 {
     rot = a.GetRotation();
 }
Beispiel #20
0
        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                //swapped?
                m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
                m_ownManifold = true;
            }
            //resultOut = new ManifoldResult();
            resultOut.SetPersistentManifold(m_manifoldPtr);

            //comment-out next line to test multi-contact generation
            //resultOut.GetPersistentManifold().ClearManifold();


            ConvexShape    min0 = body0.GetCollisionShape() as ConvexShape;
            ConvexShape    min1 = body1.GetCollisionShape() as ConvexShape;
            IndexedVector3 normalOnB;
            IndexedVector3 pointOnBWorld;

#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
            if ((min0.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
            {
                CapsuleShape capsuleA = min0 as CapsuleShape;
                CapsuleShape capsuleB = min1 as CapsuleShape;
                //IndexedVector3 localScalingA = capsuleA.GetLocalScaling();
                //IndexedVector3 localScalingB = capsuleB.GetLocalScaling();

                float threshold = m_manifoldPtr.GetContactBreakingThreshold();

                float dist = CapsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.GetHalfHeight(), capsuleA.GetRadius(),
                                                    capsuleB.GetHalfHeight(), capsuleB.GetRadius(), capsuleA.GetUpAxis(), capsuleB.GetUpAxis(),
                                                    body0.GetWorldTransform(), body1.GetWorldTransform(), threshold);

                if (dist < threshold)
                {
                    Debug.Assert(normalOnB.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON));
                    resultOut.AddContactPoint(ref normalOnB, ref pointOnBWorld, dist);
                }
                resultOut.RefreshContactPoints();
                return;
            }
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER



#if USE_SEPDISTANCE_UTIL2
            if (dispatchInfo.m_useConvexConservativeDistanceUtil)
            {
                m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(), body1.getWorldTransform());
            }

            if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance() <= 0.f)
#endif //USE_SEPDISTANCE_UTIL2

            {
                ClosestPointInput input = ClosestPointInput.Default();

                using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
                {
                    gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver);
                    //TODO: if (dispatchInfo.m_useContinuous)
                    gjkPairDetector.SetMinkowskiA(min0);
                    gjkPairDetector.SetMinkowskiB(min1);

#if USE_SEPDISTANCE_UTIL2
                    if (dispatchInfo.m_useConvexConservativeDistanceUtil)
                    {
                        input.m_maximumDistanceSquared = float.MaxValue;
                    }
                    else
#endif //USE_SEPDISTANCE_UTIL2
                    {
                        input.m_maximumDistanceSquared  = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
                        input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
                    }

                    //input.m_stackAlloc = dispatchInfo.m_stackAllocator;
                    input.m_transformA = body0.GetWorldTransform();
                    input.m_transformB = body1.GetWorldTransform();


                    if (min0.IsPolyhedral() && min1.IsPolyhedral())
                    {
                        DummyResult dummy = new DummyResult();


                        PolyhedralConvexShape polyhedronA = min0 as PolyhedralConvexShape;
                        PolyhedralConvexShape polyhedronB = min1 as PolyhedralConvexShape;
                        if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetConvexPolyhedron() != null)
                        {
                            float threshold = m_manifoldPtr.GetContactBreakingThreshold();

                            float          minDist             = float.MinValue;
                            IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);
                            bool           foundSepAxis        = true;

                            if (dispatchInfo.m_enableSatConvex)
                            {
                                foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
                                    polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
                                    body0.GetWorldTransform(),
                                    body1.GetWorldTransform(),
                                    out sepNormalWorldSpace);
                            }
                            else
                            {
#if ZERO_MARGIN
                                gjkPairDetector.SetIgnoreMargin(true);
                                gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw);
#else
                                gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif

                                float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
                                if (l2 > MathUtil.SIMD_EPSILON)
                                {
                                    sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
                                    //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
                                    minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();

#if ZERO_MARGIN
                                    foundSepAxis = true;    //gjkPairDetector.getCachedSeparatingDistance()<0.f;
#else
                                    foundSepAxis = gjkPairDetector.GetCachedSeparatingDistance() < (min0.GetMargin() + min1.GetMargin());
#endif
                                }
                            }
                            if (foundSepAxis)
                            {
                                //				printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());

                                PolyhedralContactClipping.ClipHullAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
                                                                              body0.GetWorldTransform(),
                                                                              body1.GetWorldTransform(), minDist - threshold, threshold, resultOut);
                            }
                            if (m_ownManifold)
                            {
                                resultOut.RefreshContactPoints();
                            }

                            return;
                        }
                        else
                        {
                            //we can also deal with convex versus triangle (without connectivity data)
                            if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
                            {
                                m_vertices.Clear();
                                TriangleShape tri = polyhedronB as TriangleShape;
                                m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[0]);
                                m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[1]);
                                m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[2]);

                                float          threshold           = m_manifoldPtr.GetContactBreakingThreshold();
                                IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);;
                                float          minDist             = float.MinValue;
                                float          maxDist             = threshold;

                                bool foundSepAxis = false;
                                if (false)
                                {
                                    polyhedronB.InitializePolyhedralFeatures();
                                    foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
                                        polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
                                        body0.GetWorldTransform(),
                                        body1.GetWorldTransform(),
                                        out sepNormalWorldSpace);
                                    //	 printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
                                }
                                else
                                {
#if ZERO_MARGIN
                                    gjkPairDetector.SetIgnoreMargin(true);
                                    gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw);
#else
                                    gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN

                                    float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
                                    if (l2 > MathUtil.SIMD_EPSILON)
                                    {
                                        sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
                                        //minDist = gjkPairDetector.getCachedSeparatingDistance();
                                        //maxDist = threshold;
                                        minDist      = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
                                        foundSepAxis = true;
                                    }
                                }


                                if (foundSepAxis)
                                {
                                    PolyhedralContactClipping.ClipFaceAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(),
                                                                                  body0.GetWorldTransform(), m_vertices, minDist - threshold, maxDist, resultOut);
                                }

                                if (m_ownManifold)
                                {
                                    resultOut.RefreshContactPoints();
                                }
                                return;
                            }
                        }
                    }


                    gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if USE_SEPDISTANCE_UTIL2
                    float sepDist = 0.f;
                    if (dispatchInfo.m_useConvexConservativeDistanceUtil)
                    {
                        sepDist = gjkPairDetector.getCachedSeparatingDistance();
                        if (sepDist > MathUtil.SIMD_EPSILON)
                        {
                            sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
                            //now perturbe directions to get multiple contact points
                        }
                    }
#endif //USE_SEPDISTANCE_UTIL2

                    //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects

                    //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
                    if (m_numPerturbationIterations > 0 && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
                    {
                        IndexedVector3 v0, v1;

                        IndexedVector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
                        sepNormalWorldSpace.Normalize();
                        TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1);

                        bool        perturbeA  = true;
                        const float angleLimit = 0.125f * MathUtil.SIMD_PI;
                        float       perturbeAngle;
                        float       radiusA = min0.GetAngularMotionDisc();
                        float       radiusB = min1.GetAngularMotionDisc();
                        if (radiusA < radiusB)
                        {
                            perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusA;
                            perturbeA     = true;
                        }
                        else
                        {
                            perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusB;
                            perturbeA     = false;
                        }
                        if (perturbeAngle > angleLimit)
                        {
                            perturbeAngle = angleLimit;
                        }

                        IndexedMatrix unPerturbedTransform;
                        if (perturbeA)
                        {
                            unPerturbedTransform = input.m_transformA;
                        }
                        else
                        {
                            unPerturbedTransform = input.m_transformB;
                        }

                        for (int i = 0; i < m_numPerturbationIterations; i++)
                        {
                            if (v0.LengthSquared() > MathUtil.SIMD_EPSILON)
                            {
                                IndexedQuaternion perturbeRot    = new IndexedQuaternion(v0, perturbeAngle);
                                float             iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
                                IndexedQuaternion rotq           = new IndexedQuaternion(sepNormalWorldSpace, iterationAngle);

                                if (perturbeA)
                                {
                                    input.m_transformA._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body0.GetWorldTransform()._basis);
                                    input.m_transformB        = body1.GetWorldTransform();

                                    input.m_transformB = body1.GetWorldTransform();
#if DEBUG_CONTACTS
                                    dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformA, 10.0f);
#endif //DEBUG_CONTACTS
                                }
                                else
                                {
                                    input.m_transformA        = body0.GetWorldTransform();
                                    input.m_transformB._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body1.GetWorldTransform()._basis);
#if DEBUG_CONTACTS
                                    dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformB, 10.0f);
#endif
                                }

                                PerturbedContactResult perturbedResultOut = new PerturbedContactResult(resultOut, ref input.m_transformA, ref input.m_transformB, ref unPerturbedTransform, perturbeA, dispatchInfo.getDebugDraw());
                                gjkPairDetector.GetClosestPoints(ref input, perturbedResultOut, dispatchInfo.getDebugDraw(), false);
                            }
                        }
                    }



#if USE_SEPDISTANCE_UTIL2
                    if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > MathUtil.SIMD_EPSILON))
                    {
                        m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(), sepDist, body0.getWorldTransform(), body1.getWorldTransform());
                    }
#endif //USE_SEPDISTANCE_UTIL2
                }
            }

            if (m_ownManifold)
            {
                resultOut.RefreshContactPoints();
            }
        }
        public HingeConstraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 pivotInA, ref IndexedVector3 pivotInB, ref IndexedVector3 axisInA, ref IndexedVector3 axisInB, bool useReferenceFrameA)
            : base(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA, rbB)
        {
            m_angularOnly                 = false;
            m_enableAngularMotor          = false;
            m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET;

            m_useReferenceFrameA = useReferenceFrameA;

            m_rbAFrame._origin = pivotInA;
#if     _BT_USE_CENTER_LIMIT_
            m_limit = new AngularLimit();
#endif


            m_flags = 0;

            // since no frame is given, assume this to be zero angle and just pick rb transform axis
            IndexedVector3 rbAxisA1 = rbA.GetCenterOfMassTransform()._basis.GetColumn(0);

            IndexedVector3 rbAxisA2   = IndexedVector3.Zero;
            float          projection = IndexedVector3.Dot(axisInA, rbAxisA1);
            if (projection >= 1.0f - MathUtil.SIMD_EPSILON)
            {
                rbAxisA1 = -rbA.GetCenterOfMassTransform()._basis.GetColumn(2);
                rbAxisA2 = rbA.GetCenterOfMassTransform()._basis.GetColumn(1);
            }
            else if (projection <= -1.0f + MathUtil.SIMD_EPSILON)
            {
                rbAxisA1 = rbA.GetCenterOfMassTransform()._basis.GetColumn(2);
                rbAxisA2 = rbA.GetCenterOfMassTransform()._basis.GetColumn(1);
            }
            else
            {
                rbAxisA2 = IndexedVector3.Cross(axisInA, rbAxisA1);
                rbAxisA1 = IndexedVector3.Cross(rbAxisA2, axisInA);
            }


            //m_rbAFrame._basis = new IndexedBasisMatrix(ref rbAxisA1, ref rbAxisA2, ref axisInA);
            m_rbAFrame._basis = new IndexedBasisMatrix(rbAxisA1.X, rbAxisA2.X, axisInA.X,
                                                       rbAxisA1.Y, rbAxisA2.Y, axisInA.Y,
                                                       rbAxisA1.Z, rbAxisA2.Z, axisInA.Z);

            IndexedQuaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB);
            IndexedVector3    rbAxisB1    = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1);
            IndexedVector3    rbAxisB2    = IndexedVector3.Cross(axisInB, rbAxisB1);

            m_rbBFrame._origin = pivotInB;
            //m_rbBFrame._basis = new IndexedBasisMatrix(ref rbAxisB1, ref rbAxisB2, ref axisInB);
            m_rbBFrame._basis = new IndexedBasisMatrix(rbAxisB1.X, rbAxisB2.X, axisInB.X,
                                                       rbAxisB1.Y, rbAxisB2.Y, axisInB.Y,
                                                       rbAxisB1.Z, rbAxisB2.Z, axisInB.Z);

#if !_BT_USE_CENTER_LIMIT_
            //start with free
            m_lowerLimit       = float(1.0f);
            m_upperLimit       = float(-1.0f);
            m_biasFactor       = 0.3f;
            m_relaxationFactor = 1.0f;
            m_limitSoftness    = 0.9f;
            m_solveLimit       = false;
#endif
            m_referenceSign = m_useReferenceFrameA ? -1f : 1f;
        }