Пример #1
0
        public static IndexedMatrix CreateFromQuaternion(ref IndexedQuaternion q)
        {
            IndexedMatrix i = new IndexedMatrix();

            i._basis.SetRotation(ref q);
            return(i);
        }
Пример #2
0
        public IndexedVector3 QuatRotate(IndexedQuaternion rotation, IndexedVector3 v)
        {
            IndexedQuaternion q = rotation * v;

            q *= rotation.Inverse();
            return(new IndexedVector3(q.X, q.Y, q.Z));
        }
Пример #3
0
 public EntityProperties FromTransform(uint id, IndexedMatrix startTransform)
 {
     EntityProperties ret = new EntityProperties();
     ID = id;
     Position = startTransform._origin;
     Rotation = startTransform.GetRotation();
     return ret;
 }
Пример #4
0
 public IndexedBasisMatrix(ref IndexedQuaternion q)
 {
     float d = q.LengthSquared();
     Debug.Assert(d != 0.0f);
     float s = 2.0f / d;
     float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;
     float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;
     float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;
     float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;
     _el0 = new IndexedVector3(1.0f - (yy + zz), xy - wz, xz + wy);
     _el1 = new IndexedVector3(xy + wz, 1.0f - (xx + zz), yz - wx);
     _el2 = new IndexedVector3(xz - wy, yz + wx, 1.0f - (xx + yy));
 }
Пример #5
0
        public void SetRotation(ref IndexedQuaternion q)
        {
            float d = q.LengthSquared();

            Debug.Assert(d != 0.0f);
            float s = 2.0f / d;
            float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;
            float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;
            float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;
            float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;

            SetValue(1.0f - (yy + zz), xy - wz, xz + wy,
                     xy + wz, 1.0f - (xx + zz), yz - wx,
                     xz - wy, yz + wx, 1.0f - (xx + yy));
        }
Пример #6
0
        public IndexedBasisMatrix(ref IndexedQuaternion q)
        {
            float d = q.LengthSquared();

            Debug.Assert(d != 0.0f);
            float s = 2.0f / d;
            float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;
            float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;
            float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;
            float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;

            _el0 = new IndexedVector3(1.0f - (yy + zz), xy - wz, xz + wy);
            _el1 = new IndexedVector3(xy + wz, 1.0f - (xx + zz), yz - wx);
            _el2 = new IndexedVector3(xz - wy, yz + wx, 1.0f - (xx + yy));
        }
Пример #7
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 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);

		}
Пример #9
0
 public static float Dot(IndexedQuaternion q, IndexedQuaternion q2)
 {
     return q.X * q2.X + q.Y * q2.Y + q.Z * q2.Z + q.W * q2.W;
 }
Пример #10
0
 public static IndexedQuaternion Inverse(IndexedQuaternion q)
 {
     return new IndexedQuaternion(-q.X, -q.Y, -q.Z, q.W);
 }
        /// 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);
                }
            }
        }
Пример #12
0
 public virtual void Rotate(IndexedQuaternion iq)
 {
     IndexedMatrix im = IndexedMatrix.CreateFromQuaternion(iq);
     im._origin = m_graphicsWorldTrans._origin;
     SetWorldTransform(ref im);
 }
Пример #13
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)));
 }
Пример #14
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;
        }
Пример #15
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;
	    }
Пример #16
0
 public static void GetRotation(ref IndexedBasisMatrix a, out IndexedQuaternion rot)
 {
     rot = a.GetRotation();
 }
Пример #17
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();
            }
        }
Пример #18
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;
     }
 }
Пример #19
0
    //SetFrames2(m_constraint.ptr, frameA, frameArot, frameB, frameBrot);
    internal static void SetFrames2(object pConstraint, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot)
    {
        Generic6DofConstraint constraint = pConstraint as Generic6DofConstraint;
        IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
        IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
        IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
        frame1._origin = frame1v;

        IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
        IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
        IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
        frame2._origin = frame1v;
        constraint.SetFrames(ref frame1, ref frame2);
    }
Пример #20
0
 public static IndexedQuaternion Inverse(IndexedQuaternion q)
 {
     return(new IndexedQuaternion(-q.X, -q.Y, -q.Z, q.W));
 }
Пример #21
0
 public override void SetTranslation(BulletBody pBody, Vector3 _position, Quaternion _orientation)
 {
     RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
     IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
     IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
                                                           _orientation.W);
     IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
     mat._origin = vposition;
     body.SetWorldTransform(mat);
     
 }
Пример #22
0
 public float Dot(IndexedQuaternion q)
 {
     return(X * q.X + Y * q.Y + Z * q.Z + W * q.W);
 }
        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;
        }
Пример #24
0
 public static float Dot(IndexedQuaternion q, IndexedQuaternion q2)
 {
     return(q.X * q2.X + q.Y * q2.Y + q.Z * q2.Z + q.W * q2.W);
 }
            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;
                        }

                }
            }
Пример #26
0
    public override BulletConstraint Create6DofSpringConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
                                    Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot,
                                    bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)

    {
        Generic6DofSpringConstraint constrain = null;
        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
        RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
        RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
        if (body1 != null && body2 != null)
        {
            IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
            IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
            IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
            frame1._origin = frame1v;

            IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
            IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
            IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
            frame2._origin = frame1v;

            constrain = new Generic6DofSpringConstraint(body1, body2, ref frame1, ref frame2, puseLinearReferenceFrameA);
            world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);

            constrain.CalculateTransforms();
        }

        return new BulletConstraintXNA(constrain);
    }
Пример #27
0
        public float Dot(IndexedQuaternion q)
	    {
		    return X * q.X + Y * q.Y + Z * q.Z + W * q.W;
	    }
Пример #28
0
    public override void SetTranslation(BulletBody pCollisionObject, Vector3 _position, Quaternion _orientation)
    {
        CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
        IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
        IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
                                                              _orientation.W);
        IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
        mat._origin = vposition;
        collisionObject.SetWorldTransform(mat);

    }
Пример #29
0
        public IndexedVector3 QuatRotate(IndexedQuaternion rotation, IndexedVector3 v) 
        {
	        IndexedQuaternion q = rotation * v;
	        q *= rotation.Inverse();
	        return new IndexedVector3(q.X,q.Y,q.Z);
        }
Пример #30
0
    //SetFrames(m_constraint.ptr, frameA, frameArot, frameB, frameBrot);
    public override bool SetFrames(BulletConstraint pConstraint, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot)
    {
        Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
        IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
        IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
        IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
        frame1._origin = frame1v;

        IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
        IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
        IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
        frame2._origin = frame2v;
        constraint.SetFrames(ref frame1, ref frame2);
        return true;
    }
Пример #31
0
 public override void SetCenterOfMassByPosRot(BulletBody pBody, Vector3 pos, Quaternion rot)
 {
     RigidBody body = (pBody as BulletBodyXNA).rigidBody;
     IndexedQuaternion quat = new IndexedQuaternion(rot.X, rot.Y, rot.Z,rot.W);
     IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(quat);
     mat._origin = new IndexedVector3(pos.X, pos.Y, pos.Z);
     body.SetCenterOfMassTransform( ref mat);
     /* TODO: double check this */
 }
Пример #32
0
 public void SetRotation(ref IndexedQuaternion q)
 {
     _basis.SetRotation(ref q);
 }
Пример #33
0
    public override BulletConstraint CreateConeTwistConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
                        Vector3 pframe1, Quaternion pframe1rot,
                        Vector3 pframe2, Quaternion pframe2rot,
                        bool pdisableCollisionsBetweenLinkedBodies)
    {
        ConeTwistConstraint constrain = null;
        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
        RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
        RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
        if (rb1 != null && rb2 != null)
        {
            IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
            IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
            IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
            frame1._origin = frame1v;

            IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
            IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
            IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
            frame2._origin = frame1v;

            constrain = new ConeTwistConstraint(rb1, rb2, ref frame1, ref frame2);
            world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
        }
        return new BulletConstraintXNA(constrain);
    }
Пример #34
0
    internal static void AddObjectToWorld2(object pWorld, object pBody, Vector3 _position, Quaternion _orientation)
    {
        RigidBody body = pBody as RigidBody;
        DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
        //if (!(body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE && body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE))

        world.AddRigidBody(body);
        IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
        IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
                                                              _orientation.W);
        IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
        mat._origin = vposition;
        body.SetWorldTransform(mat);
        //if (body.GetBroadphaseHandle() != null)
        //    world.UpdateSingleAabb(body);
    }
Пример #35
0
    public override BulletConstraint Create6DofConstraintFixed(BulletWorld pWorld, BulletBody pBody1,
                                        Vector3 pframe1, Quaternion pframe1rot,
                                        bool pUseLinearReferenceFrameB, bool pdisableCollisionsBetweenLinkedBodies)
    {
        DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
        RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
        IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
        IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
        IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
        frame1._origin = frame1v;

        Generic6DofConstraint consttr = new Generic6DofConstraint(body1, ref frame1, pUseLinearReferenceFrameB);
        consttr.CalculateTransforms();
        world.AddConstraint(consttr,pdisableCollisionsBetweenLinkedBodies);

        return new BulletConstraintXNA(consttr);
    }
Пример #36
0
 internal static void SetTranslation2(object pBody, Vector3 _position, Quaternion _orientation)
 {
     RigidBody body = pBody as RigidBody;
     IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
     IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
                                                           _orientation.W);
     IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
     mat._origin = vposition;
     body.SetWorldTransform(mat);
     
 }
        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();
            }
        }
Пример #38
0
    //BulletSimAPI.Create6DofConstraint2(m_world.ptr, m_body1.ptr, m_body2.ptr,frame1, frame1rot,frame2, frame2rot,useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
    internal static object Create6DofConstraint2(object pWorld, object pBody1, object pBody2, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)

    {
         DiscreteDynamicsWorld world = pWorld as DiscreteDynamicsWorld;
        RigidBody body1 = pBody1 as RigidBody;
        RigidBody body2 = pBody2 as RigidBody;
        IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
        IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
        IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
        frame1._origin = frame1v;

        IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
        IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
        IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
        frame2._origin = frame1v;

        Generic6DofConstraint consttr = new Generic6DofConstraint(body1, body2, ref frame1, ref frame2,
                                                                  puseLinearReferenceFrameA);
        consttr.CalculateTransforms();
        world.AddConstraint(consttr,pdisableCollisionsBetweenLinkedBodies);

        return consttr;
    }