示例#1
0
        public BasicDemo()
        {
            SphereBoxCollisionAlgorithm.CreateFunc boxAlgo = new SphereBoxCollisionAlgorithm.CreateFunc();
            boxAlgo.IsSwapped = true;
            CollisionDispatcher.RegisterCollisionCreateFunc(BroadphaseNativeTypes.Sphere, BroadphaseNativeTypes.Sphere, new SphereSphereCollisionAlgorithm.CreateFunc());
            CollisionDispatcher.RegisterCollisionCreateFunc(BroadphaseNativeTypes.Sphere, BroadphaseNativeTypes.Box, new SphereBoxCollisionAlgorithm.CreateFunc());
            CollisionDispatcher.RegisterCollisionCreateFunc(BroadphaseNativeTypes.Box, BroadphaseNativeTypes.Sphere, boxAlgo);

            Shapes[0] = new SphereShape(50);
            Shapes[2] = new SphereShape(HalfExtents - CollisionMargin);

            Matrix tr = Matrix.Identity;
            tr.Translation = new Vector3(0, -50, 0);
            CreateRigidBody(0, tr, Shapes[0]);

            for (int i = 0; i < NumObjects; i++)
            {
                Shapes[2].Margin = CollisionMargin;
                int colsize = 2;
                int row = (int)((i * HalfExtents * 2) / (colsize * 2 * HalfExtents));
                int row2 = row;
                int col = i % colsize - colsize / 2;
                tr.Translation = new Vector3(col * 2 * HalfExtents + (row2 % 2) * HalfExtents,
                    row * 2 * HalfExtents + HalfExtents, 0);

                CreateRigidBody(1, tr, Shapes[2]);
            }
        }
示例#2
0
 public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle)
 {
     this._sphere = sphere;
     this._triangle = triangle;
 }
示例#3
0
        public BulletXCharacter(String avName, BulletXScene parent_scene, OpenMetaverse.Vector3 pos, OpenMetaverse.Vector3 velocity,
                                OpenMetaverse.Vector3 size, OpenMetaverse.Vector3 acceleration, OpenMetaverse.Quaternion orientation)
            : base(avName)
        {
            //This fields will be removed. They're temporal
            float _sizeX = 0.5f;
            float _sizeY = 0.5f;
            float _sizeZ = 1.6f;
            //.
            _position = pos;
            _velocity = velocity;
            _size = size;
            //---
            _size.X = _sizeX;
            _size.Y = _sizeY;
            _size.Z = _sizeZ;
            //.
            _acceleration = acceleration;
            _orientation = orientation;
            _physical = true;

            float _mass = 50.0f; //This depends of avatar's dimensions
            //For RigidBody Constructor. The next values might change
            float _linearDamping = 0.0f;
            float _angularDamping = 0.0f;
            float _friction = 0.5f;
            float _restitution = 0.0f;
            Matrix _startTransform = Matrix.Identity;
            Matrix _centerOfMassOffset = Matrix.Identity;
            lock (BulletXScene.BulletXLock)
            {
                _startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(pos);
                //CollisionShape _collisionShape = new BoxShape(new MonoXnaCompactMaths.Vector3(1.0f, 1.0f, 1.60f));
                //For now, like ODE, collisionShape = sphere of radious = 1.0
                CollisionShape _collisionShape = new SphereShape(1.0f);
                DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
                Vector3 _localInertia = new Vector3();
                _collisionShape.CalculateLocalInertia(_mass, out _localInertia); //Always when mass > 0
                rigidBody =
                    new RigidBody(_mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping,
                                  _friction, _restitution);
                //rigidBody.ActivationState = ActivationState.DisableDeactivation;
                //It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
                Vector3 _vDebugTranslation;
                _vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition;
                rigidBody.Translate(_vDebugTranslation);
                parent_scene.ddWorld.AddRigidBody(rigidBody);
            }
        }
示例#4
0
        public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            //Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold

            //Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
            //col0->m_worldTransform,
            float resultFraction = 1f;

            float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared();
            float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared();

            if (squareMotA < colA.CcdSquareMotionThreshold &&
                squareMotB < colB.CcdSquareMotionThreshold)
                return resultFraction;

            if (DisableCcd)
                return 1f;

            //An adhoc way of testing the Continuous Collision Detection algorithms
            //One object is approximated as a sphere, to simplify things
            //Starting in penetration should report no time of impact
            //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
            //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)

            // Convex0 against sphere for Convex1
            {
                ConvexShape convexA = colA.CollisionShape as ConvexShape;

                SphereShape sphereB = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
                CastResult result = new CastResult();
                VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                //Simplification, one object is simplified as a sphere
                GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex);
                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
                if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
                    colB.WorldTransform, colB.InterpolationWorldTransform, result))
                {
                    //store result.m_fraction in both bodies
                    if (colA.HitFraction > result.Fraction)
                        colA.HitFraction = result.Fraction;

                    if (colB.HitFraction > result.Fraction)
                        colB.HitFraction = result.Fraction;

                    if (resultFraction > result.Fraction)
                        resultFraction = result.Fraction;
                }
            }

            // Sphere (for convex0) against Convex1
            {
                ConvexShape convexB = colB.CollisionShape as ConvexShape;

                SphereShape sphereA = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
                CastResult result = new CastResult();
                VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                ///Simplification, one object is simplified as a sphere
                GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex);
                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
                if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
                    colB.WorldTransform, colB.InterpolationWorldTransform, result))
                {
                    //store result.m_fraction in both bodies
                    if (colA.HitFraction > result.Fraction)
                        colA.HitFraction = result.Fraction;

                    if (colB.HitFraction > result.Fraction)
                        colB.HitFraction = result.Fraction;

                    if (resultFraction > result.Fraction)
                        resultFraction = result.Fraction;
                }
            }
            return resultFraction;
        }
示例#5
0
        /// <summary>
        /// cast a convex against another convex object
        /// </summary>
        /// <param name="fromA"></param>
        /// <param name="toA"></param>
        /// <param name="fromB"></param>
        /// <param name="toB"></param>
        /// <param name="result"></param>
        /// <returns></returns>
        public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
        {
            MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB);

            Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
            Matrix rayToLocalA   = MathHelper.InvertMatrix(toA) * toB;

            Matrix transformA = fromA;
            Matrix transformB = fromB;

            transformA.Translation = new Vector3(0, 0, 0);
            transformB.Translation = new Vector3(0, 0, 0);

            combined.TransformA = transformA;
            combined.TransformB = transformB;

            float radius = 0.01f;
            float lambda = 0;

            Vector3 s = rayFromLocalA.Translation;
            Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation;
            Vector3 x = s;
            Vector3 n = new Vector3();
            Vector3 c = new Vector3();

            bool  hasResult  = false;
            float lastLambda = lambda;

            IConvexPenetrationDepthSolver penSolver = null;
            Matrix identityTransform = Matrix.Identity;

            SphereShape raySphere = new SphereShape(0.0f);

            raySphere.Margin = 0.0f;

            Matrix sphereTransform = Matrix.Identity;

            sphereTransform.Translation = rayFromLocalA.Translation;

            result.DrawCoordSystem(sphereTransform);

            {
                PointCollector  pointCollector = new PointCollector();
                GjkPairDetector gjk            = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);

                GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
                input.TransformA = sphereTransform;
                input.TransformB = identityTransform;

                gjk.GetClosestPoints(input, pointCollector, null);

                hasResult = pointCollector.HasResult;

                c = pointCollector.PointInWorld;
                n = pointCollector.NormalOnBInWorld;
            }

            if (hasResult)
            {
                float dist = (c - x).Length();

                if (dist < radius)
                {
                    lastLambda = 1.0f;
                }

                while (dist > radius)
                {
                    n = x - c;
                    float dot = Vector3.Dot(n, r);

                    if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon))
                    {
                        return(false);
                    }

                    lambda = lambda - Vector3.Distance(n, n) / dot;
                    if (lambda <= lastLambda)
                    {
                        break;
                    }

                    lastLambda = lambda;

                    x = s + lambda * r;

                    sphereTransform.Translation = x;
                    result.DrawCoordSystem(sphereTransform);
                    PointCollector pointCollector = new PointCollector();

                    GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
                    GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
                    input.TransformA = sphereTransform;
                    input.TransformB = identityTransform;

                    gjk.GetClosestPoints(input, pointCollector, null);

                    if (pointCollector.HasResult)
                    {
                        if (pointCollector.Distance < 0.0f)
                        {
                            result.Fraction = lastLambda;
                            result.Normal   = n;
                            return(true);
                        }

                        c    = pointCollector.PointInWorld;
                        dist = (c - x).Length();
                    }
                    else
                    {
                        return(false);
                    }
                }

                if (lastLambda < 1.0f)
                {
                    result.Fraction = lastLambda;
                    result.Normal   = n;
                    return(true);
                }
            }

            return(false);
        }
示例#6
0
		// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
		// In a future implementation, we consider moving the ray test as a virtual method in CollisionShape.
		// This allows more customization.
		public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans,
						  CollisionObject collisionObject,
						  CollisionShape collisionShape,
						  Matrix colObjWorldTransform,
						  RayResultCallback resultCallback)
		{
			SphereShape pointShape=new SphereShape(0.0f);

			if (collisionShape.IsConvex)
			{
				CastResult castResult = new CastResult();
				castResult.Fraction = 1f;//??

				ConvexShape convexShape = collisionShape as ConvexShape;
				VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
				SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver);
				//GjkConvexCast	convexCaster(&pointShape,convexShape,&simplexSolver);
				//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);

				if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult))
				{
					//add hit
					if (castResult.Normal.LengthSquared() > 0.0001f)
					{
						castResult.Normal.Normalize();
						if (castResult.Fraction < resultCallback.ClosestHitFraction)
						{

							CollisionWorld.LocalRayResult localRayResult = new LocalRayResult
								(
									collisionObject,
									new LocalShapeInfo(),
									castResult.Normal,
									castResult.Fraction
								);

							resultCallback.AddSingleResult(localRayResult);
						}
					}
				}
				else
				{
					if (collisionShape.IsConcave)
					{

						TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape;

						Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform);

						Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject);
						Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject);

						BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh);
						rcb.HitFraction = resultCallback.ClosestHitFraction;

						Vector3 rayAabbMinLocal = rayFromLocal;
						MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal);
						Vector3 rayAabbMaxLocal = rayFromLocal;
						MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal);

						triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal);
					}
					else
					{
						//todo: use AABB tree or other BVH acceleration structure!
						if (collisionShape.IsCompound)
						{
							CompoundShape compoundShape = collisionShape as CompoundShape;
							for (int i = 0; i < compoundShape.ChildShapeCount; i++)
							{
								Matrix childTrans = compoundShape.GetChildTransform(i);
								CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
								Matrix childWorldTrans = colObjWorldTransform * childTrans;
								RayTestSingle(rayFromTrans, rayToTrans,
									collisionObject,
									childCollisionShape,
									childWorldTrans,
									resultCallback);
							}
						}
					}
				}
			}
		}
		public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
		{
			_simplexSolver.Reset();

			// compute linear and angular velocity for this interval, to interpolate
			Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3();
			TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA);
			TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB);

			float boundingRadiusA = _convexA.GetAngularMotionDisc();
			float boundingRadiusB = _convexB.GetAngularMotionDisc();

			float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA +
				angVelB.Length() * boundingRadiusB;

			float radius = 0.001f;

			float lambda = 0f;
			Vector3 v = new Vector3(1f, 0f, 0f);

			int maxIter = MaxIterations;

			Vector3 n = new Vector3();
			bool hasResult = false;
			Vector3 c;

			float lastLambda = lambda;
			//float epsilon = 0.001f;

			int numIter = 0;
			//first solution, using GJK


			Matrix identityTrans = Matrix.Identity;

			SphereShape raySphere = new SphereShape(0f);
			raySphere.Margin=0f;


			//result.drawCoordSystem(sphereTr);

			PointCollector pointCollector1 = new PointCollector();

			GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
			GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();

			//we don't use margins during CCD
			gjk.setIgnoreMargin(true);

			input.TransformA = fromA;
			input.TransformB = fromB;

			DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
			gjk.GetClosestPoints(input, r, null);

			hasResult = pointCollector1.HasResult;
			c = pointCollector1.PointInWorld;

			if (hasResult)
			{
				float dist;
				dist = pointCollector1.Distance;
				n = pointCollector1.NormalOnBInWorld;

				//not close enough
				while (dist > radius)
				{
					numIter++;
					if (numIter > maxIter)
						return false; //todo: report a failure

					float dLambda = 0f;

					//calculate safe moving fraction from distance / (linear+rotational velocity)

					//float clippedDist  = GEN_min(angularConservativeRadius,dist);
					//float clippedDist  = dist;

					float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n);

					dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);

					lambda = lambda + dLambda;

					if (lambda > 1f) return false;
					if (lambda < 0f) return false;

					//todo: next check with relative epsilon
					if (lambda <= lastLambda)
						break;
					lastLambda = lambda;


					//interpolate to next lambda
					Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans;

					TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA);
					TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB);

					relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA);

					result.DebugDraw(lambda);

					PointCollector pointCollector = new PointCollector();
					gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
					input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
					input.TransformA = interpolatedTransA;
					input.TransformB = interpolatedTransB;

					// !!!!!!!!!!
					r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
					gjk.GetClosestPoints(input, r, null);

					if (pointCollector.HasResult)
					{
						if (pointCollector.Distance < 0f)
						{
							//degenerate ?!
							result.Fraction = lastLambda;
							result.Normal = n;
							return true;
						}
						c = pointCollector.PointInWorld;

						dist = pointCollector.Distance;
					}
					else
					{
						//??
						return false;
					}

				}

				result.Fraction = lambda;
				result.Normal = n;
				return true;
			}

			return false;
		}
			public void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex)
			{
				//do a swept sphere for now
				Matrix ident = Matrix.Identity;
				CastResult castResult = new CastResult();
				castResult.Fraction = _hitFraction;
				SphereShape pointShape = new SphereShape(_ccdSphereRadius);
				TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]);
				VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
				SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver);
				//GjkConvexCast	convexCaster(&pointShape,convexShape,&simplexSolver);
				//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
				//local space?

				if (convexCaster.CalcTimeOfImpact(_ccdSphereFromTrans, _ccdSphereToTrans,
					ident, ident, castResult))
				{
					if (_hitFraction > castResult.Fraction)
						_hitFraction = castResult.Fraction;
				}
			}
示例#9
0
 public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle)
 {
     this._sphere   = sphere;
     this._triangle = triangle;
 }
示例#10
0
        /// <summary>
        /// cast a convex against another convex object
        /// </summary>
        /// <param name="fromA"></param>
        /// <param name="toA"></param>
        /// <param name="fromB"></param>
        /// <param name="toB"></param>
        /// <param name="result"></param>
        /// <returns></returns>
		public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
		{
			MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB);

            Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
            Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB;

			Matrix transformA = fromA;
			Matrix transformB = fromB;

			transformA.Translation = new Vector3(0, 0, 0);
			transformB.Translation = new Vector3(0, 0, 0);

			combined.TransformA = transformA;
			combined.TransformB = transformB;

			float radius = 0.01f;
			float lambda = 0;

			Vector3 s = rayFromLocalA.Translation;
			Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation;
			Vector3 x = s;
			Vector3 n = new Vector3();
			Vector3 c = new Vector3();

			bool hasResult = false;
			float lastLambda = lambda;

			IConvexPenetrationDepthSolver penSolver = null;
			Matrix identityTransform = Matrix.Identity;

			SphereShape raySphere = new SphereShape(0.0f);
			raySphere.Margin=0.0f;

			Matrix sphereTransform = Matrix.Identity;
			sphereTransform.Translation = rayFromLocalA.Translation;

			result.DrawCoordSystem(sphereTransform);

			{
				PointCollector pointCollector = new PointCollector();
				GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);

				GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
				input.TransformA = sphereTransform;
				input.TransformB = identityTransform;

				gjk.GetClosestPoints(input, pointCollector, null);

				hasResult = pointCollector.HasResult;

				c = pointCollector.PointInWorld;
				n = pointCollector.NormalOnBInWorld;
			}

			if (hasResult)
			{
				float dist = (c - x).Length();

				if (dist < radius)
				{
					lastLambda = 1.0f;
				}

				while (dist > radius)
				{
					n = x - c;
					float dot = Vector3.Dot(n, r);

					if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false;

					lambda = lambda - Vector3.Distance(n, n) / dot;
					if (lambda <= lastLambda) break;

					lastLambda = lambda;

					x = s + lambda * r;

					sphereTransform.Translation = x;
					result.DrawCoordSystem(sphereTransform);
					PointCollector pointCollector = new PointCollector();

					GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
					GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
					input.TransformA = sphereTransform;
					input.TransformB = identityTransform;

					gjk.GetClosestPoints(input, pointCollector, null);

					if (pointCollector.HasResult)
					{
						if (pointCollector.Distance < 0.0f)
						{
							result.Fraction = lastLambda;
							result.Normal = n;
							return true;
						}

						c = pointCollector.PointInWorld;
						dist = (c - x).Length();
					}
					else
					{
						return false;
					}
				}

				if (lastLambda < 1.0f)
				{
					result.Fraction = lastLambda;
					result.Normal = n;
					return true;
				}
			}

			return false;
		}
示例#11
0
        /// <summary>
        /// Reinitializes physics.
        /// </summary>
        public void ResetPhysics()
        {
            _collisionDispatcher = new CollisionDispatcher();

            if (_useSweepAndPrune)
            {
                Vector3 worldAabbMin = new Vector3(-10000, -10000, -10000);
                Vector3 worldAabbMax = new Vector3(10000, 10000, 10000);
                const int maxProxies = 32766;
                _broadphase = new AxisSweep3(worldAabbMin, worldAabbMax, maxProxies);
            }
            else
                _broadphase = new SimpleBroadphase();

            _solver = new SequentialImpulseConstraintSolver();
            _world = new DiscreteDynamicsWorld(_collisionDispatcher, _broadphase, _solver);

            //world.setConstraintSolver(solver);
            _world.Gravity = new Vector3(0, -10, 0);

            _shapePtr = new CollisionShape[4];
            _shapePtr[0] = new BoxShape(new Vector3(50, 10, 50));
            _shapePtr[1] = new CylinderShape(new Vector3(_cubeHalfExtent - _collisionMargin, _cubeHalfExtent - _collisionMargin, _cubeHalfExtent - _collisionMargin));
            _shapePtr[2] = new SphereShape(_cubeHalfExtent);
            _shapePtr[3] = new BoxShape(new Vector3(_cubeHalfExtent, _cubeHalfExtent, _cubeHalfExtent));

            _shapeIndex = new int[_maxNumObjects];
            Matrix tr = Matrix.Identity;

            for (int i = 0; i < _numObjects; i++)
            {
                if (i > 0)
                    // set shape
                    _shapeIndex[i] = 1;
                else
                    _shapeIndex[i] = 0;
            }

            GC.Collect();
        }
        public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
        {
            _simplexSolver.Reset();

            // compute linear and angular velocity for this interval, to interpolate
            Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3();

            TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA);
            TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB);

            float boundingRadiusA = _convexA.GetAngularMotionDisc();
            float boundingRadiusB = _convexB.GetAngularMotionDisc();

            float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA +
                                                angVelB.Length() * boundingRadiusB;

            float radius = 0.001f;

            float   lambda = 0f;
            Vector3 v      = new Vector3(1f, 0f, 0f);

            int maxIter = MaxIterations;

            Vector3 n         = new Vector3();
            bool    hasResult = false;
            Vector3 c;

            float lastLambda = lambda;
            //float epsilon = 0.001f;

            int numIter = 0;
            //first solution, using GJK


            Matrix identityTrans = Matrix.Identity;

            SphereShape raySphere = new SphereShape(0f);

            raySphere.Margin = 0f;


            //result.drawCoordSystem(sphereTr);

            PointCollector pointCollector1 = new PointCollector();

            GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);

            GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();

            //we don't use margins during CCD
            gjk.setIgnoreMargin(true);

            input.TransformA = fromA;
            input.TransformB = fromB;

            DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
            gjk.GetClosestPoints(input, r, null);

            hasResult = pointCollector1.HasResult;
            c         = pointCollector1.PointInWorld;

            if (hasResult)
            {
                float dist;
                dist = pointCollector1.Distance;
                n    = pointCollector1.NormalOnBInWorld;

                //not close enough
                while (dist > radius)
                {
                    numIter++;
                    if (numIter > maxIter)
                    {
                        return(false);                        //todo: report a failure
                    }
                    float dLambda = 0f;

                    //calculate safe moving fraction from distance / (linear+rotational velocity)

                    //float clippedDist  = GEN_min(angularConservativeRadius,dist);
                    //float clippedDist  = dist;

                    float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n);

                    dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);

                    lambda = lambda + dLambda;

                    if (lambda > 1f)
                    {
                        return(false);
                    }
                    if (lambda < 0f)
                    {
                        return(false);
                    }

                    //todo: next check with relative epsilon
                    if (lambda <= lastLambda)
                    {
                        break;
                    }
                    lastLambda = lambda;


                    //interpolate to next lambda
                    Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans;

                    TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA);
                    TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB);

                    relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA);

                    result.DebugDraw(lambda);

                    PointCollector pointCollector = new PointCollector();
                    gjk              = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
                    input            = new DiscreteCollisionDetectorInterface.ClosestPointInput();
                    input.TransformA = interpolatedTransA;
                    input.TransformB = interpolatedTransB;

                    // !!!!!!!!!!
                    r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
                    gjk.GetClosestPoints(input, r, null);

                    if (pointCollector.HasResult)
                    {
                        if (pointCollector.Distance < 0f)
                        {
                            //degenerate ?!
                            result.Fraction = lastLambda;
                            result.Normal   = n;
                            return(true);
                        }
                        c = pointCollector.PointInWorld;

                        dist = pointCollector.Distance;
                    }
                    else
                    {
                        //??
                        return(false);
                    }
                }

                result.Fraction = lambda;
                result.Normal   = n;
                return(true);
            }

            return(false);
        }
示例#13
0
        // rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
        // In a future implementation, we consider moving the ray test as a virtual method in CollisionShape.
        // This allows more customization.
        public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans,
                                         CollisionObject collisionObject,
                                         CollisionShape collisionShape,
                                         Matrix colObjWorldTransform,
                                         RayResultCallback resultCallback)
        {
            SphereShape pointShape = new SphereShape(0.0f);

            if (collisionShape.IsConvex)
            {
                CastResult castResult = new CastResult();
                castResult.Fraction = 1f;                //??

                ConvexShape          convexShape   = collisionShape as ConvexShape;
                VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
                SubsimplexConvexCast convexCaster  = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver);
                //GjkConvexCast	convexCaster(&pointShape,convexShape,&simplexSolver);
                //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);

                if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult))
                {
                    //add hit
                    if (castResult.Normal.LengthSquared() > 0.0001f)
                    {
                        castResult.Normal.Normalize();
                        if (castResult.Fraction < resultCallback.ClosestHitFraction)
                        {
                            CollisionWorld.LocalRayResult localRayResult = new LocalRayResult
                                                                           (
                                collisionObject,
                                new LocalShapeInfo(),
                                castResult.Normal,
                                castResult.Fraction
                                                                           );

                            resultCallback.AddSingleResult(localRayResult);
                        }
                    }
                }
                else
                {
                    if (collisionShape.IsConcave)
                    {
                        TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape;

                        Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform);

                        Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject);
                        Vector3 rayToLocal   = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject);

                        BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh);
                        rcb.HitFraction = resultCallback.ClosestHitFraction;

                        Vector3 rayAabbMinLocal = rayFromLocal;
                        MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal);
                        Vector3 rayAabbMaxLocal = rayFromLocal;
                        MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal);

                        triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal);
                    }
                    else
                    {
                        //todo: use AABB tree or other BVH acceleration structure!
                        if (collisionShape.IsCompound)
                        {
                            CompoundShape compoundShape = collisionShape as CompoundShape;
                            for (int i = 0; i < compoundShape.ChildShapeCount; i++)
                            {
                                Matrix         childTrans          = compoundShape.GetChildTransform(i);
                                CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
                                Matrix         childWorldTrans     = colObjWorldTransform * childTrans;
                                RayTestSingle(rayFromTrans, rayToTrans,
                                              collisionObject,
                                              childCollisionShape,
                                              childWorldTrans,
                                              resultCallback);
                            }
                        }
                    }
                }
            }
        }
示例#14
0
        public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            //Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold

            //Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
            //col0->m_worldTransform,
            float resultFraction = 1f;

            float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared();
            float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared();

            if (squareMotA < colA.CcdSquareMotionThreshold &&
                squareMotB < colB.CcdSquareMotionThreshold)
            {
                return(resultFraction);
            }

            if (DisableCcd)
            {
                return(1f);
            }

            //An adhoc way of testing the Continuous Collision Detection algorithms
            //One object is approximated as a sphere, to simplify things
            //Starting in penetration should report no time of impact
            //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
            //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)

            // Convex0 against sphere for Convex1
            {
                ConvexShape convexA = colA.CollisionShape as ConvexShape;

                SphereShape          sphereB        = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
                CastResult           result         = new CastResult();
                VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                //Simplification, one object is simplified as a sphere
                GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex);
                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
                if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
                                          colB.WorldTransform, colB.InterpolationWorldTransform, result))
                {
                    //store result.m_fraction in both bodies
                    if (colA.HitFraction > result.Fraction)
                    {
                        colA.HitFraction = result.Fraction;
                    }

                    if (colB.HitFraction > result.Fraction)
                    {
                        colB.HitFraction = result.Fraction;
                    }

                    if (resultFraction > result.Fraction)
                    {
                        resultFraction = result.Fraction;
                    }
                }
            }

            // Sphere (for convex0) against Convex1
            {
                ConvexShape convexB = colB.CollisionShape as ConvexShape;

                SphereShape          sphereA        = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
                CastResult           result         = new CastResult();
                VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                ///Simplification, one object is simplified as a sphere
                GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex);
                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
                if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
                                          colB.WorldTransform, colB.InterpolationWorldTransform, result))
                {
                    //store result.m_fraction in both bodies
                    if (colA.HitFraction > result.Fraction)
                    {
                        colA.HitFraction = result.Fraction;
                    }

                    if (colB.HitFraction > result.Fraction)
                    {
                        colB.HitFraction = result.Fraction;
                    }

                    if (resultFraction > result.Fraction)
                    {
                        resultFraction = result.Fraction;
                    }
                }
            }
            return(resultFraction);
        }