示例#1
0
        public override void Update(GameTime gameTime)
        {
            // If <Space> is pressed, we fire a shot.
            if (InputService.IsPressed(Keys.Space, true))
            {
                // Get a random angle and a random distance from the target.
                float angle    = _angleDistribution.Next(_random);
                float distance = _distanceDistribution.Next(_random);

                // Create a vector v with the length of distance.
                Vector3F v = new Vector3F(0, distance, 0);

                // Rotate v.
                QuaternionF rotation = QuaternionF.CreateRotationZ(MathHelper.ToRadians(angle));
                v = rotation.Rotate(v);

                // Draw a small cross for the hit.
                var debugRenderer = GraphicsScreen.DebugRenderer2D;
                debugRenderer.DrawLine(
                    _center + v + new Vector3F(-10, -10, 0),
                    _center + v + new Vector3F(10, 10, 0),
                    Color.Black,
                    true);
                debugRenderer.DrawLine(
                    _center + v + new Vector3F(10, -10, 0),
                    _center + v + new Vector3F(-10, 10, 0),
                    Color.Black, true);
            }

            base.Update(gameTime);
        }
示例#2
0
        /// <summary>
        /// Called when a mesh should be generated for the shape.
        /// </summary>
        /// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
        /// <param name="iterationLimit">The iteration limit.</param>
        /// <returns>The triangle mesh for this shape.</returns>
        protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
        {
            // Estimate required segment angle for given accuracy.
            // (Easy to derive from simple drawing of a circle segment with a triangle used to represent
            // the segment.)
            float alpha            = (float)Math.Acos((_radius - absoluteDistanceThreshold) / _radius) * 2;
            int   numberOfSegments = (int)Math.Ceiling(ConstantsF.TwoPi / alpha);

            alpha = ConstantsF.TwoPi / numberOfSegments;

            Vector3F    r0       = new Vector3F(_radius, 0, 0);
            QuaternionF rotation = QuaternionF.CreateRotationZ(alpha);

            TriangleMesh mesh = new TriangleMesh();

            for (int i = 1; i <= numberOfSegments; i++)
            {
                Vector3F r1 = rotation.Rotate(r0);

                mesh.Add(new Triangle
                {
                    Vertex0 = Vector3F.Zero,
                    Vertex1 = r0,
                    Vertex2 = r1,
                }, true);
                r0 = r1;
            }

            return(mesh);
        }
        public override void Update(GameTime gameTime)
        {
            _debugRenderer.Clear();

            if (_avatarPose == null)
            {
                // Must wait till renderer is ready. Before that we do not get skeleton info.
                if (_avatarRenderer.State == AvatarRendererState.Ready)
                {
                    // Create AvatarPose.
                    _avatarPose = new AvatarPose(_avatarRenderer);

                    // A 'bone transform' is the transformation of a bone relative to its bind pose.
                    // Bone transforms define the pose of a skeleton.

                    // Rotate arm of avatar.
                    SkeletonPose skeletonPose  = _avatarPose.SkeletonPose;
                    int          shoulderIndex = skeletonPose.Skeleton.GetIndex("ShoulderLeft");
                    skeletonPose.SetBoneTransform(shoulderIndex, new SrtTransform(QuaternionF.CreateRotationZ(-0.9f)));

                    // The class SkeletonHelper provides some useful extension methods.
                    // One is SetBoneRotationAbsolute() which sets the orientation of a bone relative
                    // to model space.
                    // Rotate elbow to make the lower arm point forward.
                    int elbowIndex = skeletonPose.Skeleton.GetIndex("ElbowLeft");
                    SkeletonHelper.SetBoneRotationAbsolute(skeletonPose, elbowIndex, QuaternionF.CreateRotationY(ConstantsF.PiOver2));

                    // Draw avatar skeleton for debugging.
                    _debugRenderer.DrawSkeleton(skeletonPose, _pose, Vector3F.One, 0.02f, Color.Orange, true);
                }
            }
        }
示例#4
0
        public void CreateRotationZ()
        {
            float       angle = 0.3f;
            QuaternionF q     = QuaternionF.CreateRotation(Vector3F.UnitZ, angle);
            QuaternionF qz    = QuaternionF.CreateRotationZ(angle);

            Assert.AreEqual(q, qz);
        }
示例#5
0
        public void MultiplyOperator()
        {
            Pose p1 = new Pose(new Vector3F(1, 2, 3), QuaternionF.CreateRotationY(0.3f));
            Pose p2 = new Pose(new Vector3F(-4, 5, -6), QuaternionF.CreateRotationZ(-0.1f));

            Assert.IsTrue(Vector4F.AreNumericallyEqual(
                              p1.ToMatrix44F() * p2.ToMatrix44F() * new Vector4F(1, 2, 3, 1),
                              p1 * p2 * new Vector4F(1, 2, 3, 1)));
        }
示例#6
0
        public void Interpolate()
        {
            Pose p1 = new Pose(new Vector3F(1, 2, 3), QuaternionF.CreateRotationY(0.3f));
            Pose p2 = new Pose(new Vector3F(-4, 5, -6), QuaternionF.CreateRotationZ(-0.1f));

            Assert.IsTrue(Vector3F.AreNumericallyEqual(p1.Position, Pose.Interpolate(p1, p2, 0).Position));
            Assert.IsTrue(Matrix33F.AreNumericallyEqual(p1.Orientation, Pose.Interpolate(p1, p2, 0).Orientation));

            Assert.IsTrue(Vector3F.AreNumericallyEqual(p2.Position, Pose.Interpolate(p1, p2, 1).Position));
            Assert.IsTrue(Matrix33F.AreNumericallyEqual(p2.Orientation, Pose.Interpolate(p1, p2, 1).Orientation));

            Assert.IsTrue(Vector3F.AreNumericallyEqual(InterpolationHelper.Lerp(p1.Position, p2.Position, 0.3f), Pose.Interpolate(p1, p2, 0.3f).Position));
            Assert.IsTrue(
                QuaternionF.AreNumericallyEqual(
                    InterpolationHelper.Lerp(QuaternionF.CreateRotation(p1.Orientation), QuaternionF.CreateRotation(p2.Orientation), 0.3f),
                    QuaternionF.CreateRotation(Pose.Interpolate(p1, p2, 0.3f).Orientation)));
        }
        /// <summary>
        /// Initializes the ragdoll for the given skeleton pose.
        /// </summary>
        /// <param name="skeletonPose">The skeleton pose.</param>
        /// <param name="ragdoll">The ragdoll.</param>
        /// <param name="simulation">The simulation in which the ragdoll will be used.</param>
        /// <param name="scale">A scaling factor to scale the size of the ragdoll.</param>
        public static void Create(SkeletonPose skeletonPose, Ragdoll ragdoll, Simulation simulation, float scale)
        {
            var skeleton = skeletonPose.Skeleton;

            const float totalMass      = 80; // The total mass of the ragdoll.
            const int   numberOfBodies = 17;

            // Get distance from foot to head as a measure for the size of the ragdoll.
            int head               = skeleton.GetIndex("Head");
            int footLeft           = skeleton.GetIndex("L_Ankle1");
            var headPosition       = skeletonPose.GetBonePoseAbsolute(head).Translation;
            var footPosition       = skeletonPose.GetBonePoseAbsolute(footLeft).Translation;
            var headToFootDistance = (headPosition - footPosition).Length;

            // We use the same mass properties for all bodies. This is not realistic but more stable
            // because large mass differences or thin bodies (arms!) are less stable.
            // We use the mass properties of sphere proportional to the size of the model.
            var massFrame = MassFrame.FromShapeAndMass(new SphereShape(headToFootDistance / 8), Vector3F.One, totalMass / numberOfBodies, 0.1f, 1);

            var material = new UniformMaterial();

            #region ----- Add Bodies and Body Offsets -----

            var numberOfBones = skeleton.NumberOfBones;
            ragdoll.Bodies.AddRange(Enumerable.Repeat <RigidBody>(null, numberOfBones));
            ragdoll.BodyOffsets.AddRange(Enumerable.Repeat(Pose.Identity, numberOfBones));

            var pelvis = skeleton.GetIndex("Pelvis");
            ragdoll.Bodies[pelvis]      = new RigidBody(new BoxShape(0.3f * scale, 0.4f * scale, 0.55f * scale), massFrame, material);
            ragdoll.BodyOffsets[pelvis] = new Pose(new Vector3F(0, 0, 0));

            var backLower = skeleton.GetIndex("Spine");
            ragdoll.Bodies[backLower]      = new RigidBody(new BoxShape(0.36f * scale, 0.4f * scale, 0.55f * scale), massFrame, material);
            ragdoll.BodyOffsets[backLower] = new Pose(new Vector3F(0.18f * scale, 0, 0));

            var backUpper = skeleton.GetIndex("Spine2");
            ragdoll.Bodies[backUpper]      = new RigidBody(new BoxShape(0.5f * scale, 0.4f * scale, 0.65f * scale), massFrame, material);
            ragdoll.BodyOffsets[backUpper] = new Pose(new Vector3F(0.25f * scale, 0, 0));

            var neck = skeleton.GetIndex("Neck");
            ragdoll.Bodies[neck]      = new RigidBody(new CapsuleShape(0.12f * scale, 0.3f * scale), massFrame, material);
            ragdoll.BodyOffsets[neck] = new Pose(new Vector3F(0.15f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
            ragdoll.Bodies[neck].CollisionObject.Enabled = false;

            ragdoll.Bodies[head]      = new RigidBody(new SphereShape(0.2f * scale), massFrame, material);
            ragdoll.BodyOffsets[head] = new Pose(new Vector3F(0.15f * scale, 0.02f * scale, 0));

            var armUpperLeft = skeleton.GetIndex("L_UpperArm");
            ragdoll.Bodies[armUpperLeft]      = new RigidBody(new CapsuleShape(0.12f * scale, 0.6f * scale), massFrame, material);
            ragdoll.BodyOffsets[armUpperLeft] = new Pose(new Vector3F(0.2f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));

            var armLowerLeft = skeleton.GetIndex("L_Forearm");
            ragdoll.Bodies[armLowerLeft]      = new RigidBody(new CapsuleShape(0.08f * scale, 0.5f * scale), massFrame, material);
            ragdoll.BodyOffsets[armLowerLeft] = new Pose(new Vector3F(0.2f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));

            var handLeft = skeleton.GetIndex("L_Hand");
            ragdoll.Bodies[handLeft]      = new RigidBody(new BoxShape(0.2f * scale, 0.06f * scale, 0.15f * scale), massFrame, material);
            ragdoll.BodyOffsets[handLeft] = new Pose(new Vector3F(0.1f * scale, 0, 0));

            var armUpperRight = skeleton.GetIndex("R_UpperArm");
            ragdoll.Bodies[armUpperRight]      = new RigidBody(new CapsuleShape(0.12f * scale, 0.6f * scale), massFrame, material);
            ragdoll.BodyOffsets[armUpperRight] = new Pose(new Vector3F(0.2f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));

            var armLowerRight = skeleton.GetIndex("R_Forearm");
            ragdoll.Bodies[armLowerRight]      = new RigidBody(new CapsuleShape(0.08f * scale, 0.5f * scale), massFrame, material);
            ragdoll.BodyOffsets[armLowerRight] = new Pose(new Vector3F(0.2f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));

            var handRight = skeleton.GetIndex("R_Hand");
            ragdoll.Bodies[handRight]      = new RigidBody(new BoxShape(0.2f * scale, 0.06f * scale, 0.15f * scale), massFrame, material);
            ragdoll.BodyOffsets[handRight] = new Pose(new Vector3F(0.1f * scale, 0, 0));

            var legUpperLeft = skeleton.GetIndex("L_Thigh1");
            ragdoll.Bodies[legUpperLeft]      = new RigidBody(new CapsuleShape(0.16f * scale, 0.8f * scale), massFrame, material);
            ragdoll.BodyOffsets[legUpperLeft] = new Pose(new Vector3F(0.4f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));

            var legLowerLeft = skeleton.GetIndex("L_Knee2");
            ragdoll.Bodies[legLowerLeft]      = new RigidBody(new CapsuleShape(0.12f * scale, 0.65f * scale), massFrame, material);
            ragdoll.BodyOffsets[legLowerLeft] = new Pose(new Vector3F(0.32f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));

            //var footLeft = skeleton.GetIndex("L_Ankle1");
            ragdoll.Bodies[footLeft]      = new RigidBody(new BoxShape(0.20f * scale, 0.5f * scale, 0.3f * scale), massFrame, material);
            ragdoll.BodyOffsets[footLeft] = new Pose(new Vector3F(0.16f * scale, 0.15f * scale, 0));

            var legUpperRight = skeleton.GetIndex("R_Thigh");
            ragdoll.Bodies[legUpperRight]      = new RigidBody(new CapsuleShape(0.16f * scale, 0.8f * scale), massFrame, material);
            ragdoll.BodyOffsets[legUpperRight] = new Pose(new Vector3F(0.4f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));

            var legLowerRight = skeleton.GetIndex("R_Knee");
            ragdoll.Bodies[legLowerRight]      = new RigidBody(new CapsuleShape(0.12f * scale, 0.65f * scale), massFrame, material);
            ragdoll.BodyOffsets[legLowerRight] = new Pose(new Vector3F(0.32f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));

            var footRight = skeleton.GetIndex("R_Ankle");
            ragdoll.Bodies[footRight]      = new RigidBody(new BoxShape(0.20f * scale, 0.5f * scale, 0.3f * scale), massFrame, material);
            ragdoll.BodyOffsets[footRight] = new Pose(new Vector3F(0.16f * scale, 0.15f * scale, 0));
            #endregion

            #region ----- Set Collision Filters -----

            // Collisions between connected bodies will be disabled in AddJoint(). (A BallJoint
            // has a property CollisionEnabled which decides whether connected bodies can
            // collide.)
            // But we need to disable some more collision between bodies that are not directly
            // connected but still too close to each other.
            var filter = (ICollisionFilter)simulation.CollisionDomain.CollisionDetection.CollisionFilter;
            filter.Set(ragdoll.Bodies[backUpper].CollisionObject, ragdoll.Bodies[head].CollisionObject, false);
            filter.Set(ragdoll.Bodies[armUpperRight].CollisionObject, ragdoll.Bodies[backLower].CollisionObject, false);
            filter.Set(ragdoll.Bodies[armUpperLeft].CollisionObject, ragdoll.Bodies[backLower].CollisionObject, false);
            filter.Set(ragdoll.Bodies[legUpperLeft].CollisionObject, ragdoll.Bodies[legUpperRight].CollisionObject, false);
            #endregion

            #region ----- Add Joints -----

            AddJoint(skeletonPose, ragdoll, pelvis, backLower);
            AddJoint(skeletonPose, ragdoll, backLower, backUpper);
            AddJoint(skeletonPose, ragdoll, backUpper, neck);
            AddJoint(skeletonPose, ragdoll, neck, head);
            AddJoint(skeletonPose, ragdoll, backUpper, armUpperLeft);
            AddJoint(skeletonPose, ragdoll, armUpperLeft, armLowerLeft);
            AddJoint(skeletonPose, ragdoll, armLowerLeft, handLeft);
            AddJoint(skeletonPose, ragdoll, backUpper, armUpperRight);
            AddJoint(skeletonPose, ragdoll, armUpperRight, armLowerRight);
            AddJoint(skeletonPose, ragdoll, armLowerRight, handRight);
            AddJoint(skeletonPose, ragdoll, pelvis, legUpperLeft);
            AddJoint(skeletonPose, ragdoll, legUpperLeft, legLowerLeft);
            AddJoint(skeletonPose, ragdoll, legLowerLeft, footLeft);
            AddJoint(skeletonPose, ragdoll, pelvis, legUpperRight);
            AddJoint(skeletonPose, ragdoll, legUpperRight, legLowerRight);
            AddJoint(skeletonPose, ragdoll, legLowerRight, footRight);
            #endregion

            #region ----- Add Limits -----

            // Choosing limits is difficult.
            // We create hinge limits with AngularLimits in the back and in the knee.
            // For all other joints we use TwistSwingLimits with symmetric cones.

            AddAngularLimit(skeletonPose, ragdoll, pelvis, backLower, new Vector3F(0, 0, -0.3f), new Vector3F(0, 0, 0.3f));
            AddAngularLimit(skeletonPose, ragdoll, backLower, backUpper, new Vector3F(0, 0, -0.3f), new Vector3F(0, 0, 0.4f));
            AddAngularLimit(skeletonPose, ragdoll, backUpper, neck, new Vector3F(0, 0, -0.3f), new Vector3F(0, 0, 0.3f));
            AddTwistSwingLimit(ragdoll, neck, head, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.1f, -0.5f, -0.7f), new Vector3F(0.1f, 0.5f, 0.7f));

            var parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(backUpper).Inverse;
            var childBindPoseAbsolute  = (Pose)skeleton.GetBindPoseAbsoluteInverse(armUpperLeft).Inverse;
            var bindPoseRelative       = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute;
            AddTwistSwingLimit(ragdoll, backUpper, armUpperLeft, bindPoseRelative.Orientation * Matrix33F.CreateRotationY(-0.5f) * Matrix33F.CreateRotationZ(-0.5f), Matrix33F.Identity, new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(0.7f, 1.2f, 1.2f));

            AddTwistSwingLimit(ragdoll, armUpperLeft, armLowerLeft, Matrix33F.CreateRotationZ(-1.2f), Matrix33F.Identity, new Vector3F(-0.3f, -1.2f, -1.2f), new Vector3F(0.3f, 1.2f, 1.2f));
            AddTwistSwingLimit(ragdoll, armLowerLeft, handLeft, Matrix33F.Identity, Matrix33F.CreateRotationX(+ConstantsF.PiOver2), new Vector3F(-0.3f, -0.7f, -0.7f), new Vector3F(0.3f, 0.7f, 0.7f));

            parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(backUpper).Inverse;
            childBindPoseAbsolute  = (Pose)skeleton.GetBindPoseAbsoluteInverse(armUpperRight).Inverse;
            bindPoseRelative       = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute;
            AddTwistSwingLimit(ragdoll, backUpper, armUpperRight, bindPoseRelative.Orientation * Matrix33F.CreateRotationY(0.5f) * Matrix33F.CreateRotationZ(-0.5f), Matrix33F.Identity, new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(0.7f, 1.2f, 1.2f));

            AddTwistSwingLimit(ragdoll, armUpperRight, armLowerRight, Matrix33F.CreateRotationZ(-1.2f), Matrix33F.Identity, new Vector3F(-0.3f, -1.2f, -1.2f), new Vector3F(0.3f, 1.2f, 1.2f));
            AddTwistSwingLimit(ragdoll, armLowerRight, handRight, Matrix33F.Identity, Matrix33F.CreateRotationX(-ConstantsF.PiOver2), new Vector3F(-0.3f, -0.7f, -0.7f), new Vector3F(0.3f, 0.7f, 0.7f));

            parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(pelvis).Inverse;
            childBindPoseAbsolute  = (Pose)skeleton.GetBindPoseAbsoluteInverse(legUpperLeft).Inverse;
            bindPoseRelative       = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute;
            AddTwistSwingLimit(ragdoll, pelvis, legUpperLeft, bindPoseRelative.Orientation * Matrix33F.CreateRotationZ(1.2f), Matrix33F.Identity, new Vector3F(-0.1f, -0.7f, -1.5f), new Vector3F(+0.1f, +0.7f, +1.5f));

            AddAngularLimit(skeletonPose, ragdoll, legUpperLeft, legLowerLeft, new Vector3F(0, 0, -2.2f), new Vector3F(0, 0, 0.0f));
            AddTwistSwingLimit(ragdoll, legLowerLeft, footLeft, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.1f, -0.3f, -0.7f), new Vector3F(0.1f, 0.3f, 0.7f));

            parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(pelvis).Inverse;
            childBindPoseAbsolute  = (Pose)skeleton.GetBindPoseAbsoluteInverse(legUpperRight).Inverse;
            bindPoseRelative       = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute;
            AddTwistSwingLimit(ragdoll, pelvis, legUpperRight, bindPoseRelative.Orientation * Matrix33F.CreateRotationZ(1.2f), Matrix33F.Identity, new Vector3F(-0.1f, -0.7f, -1.5f), new Vector3F(+0.1f, +0.7f, +1.5f));

            AddAngularLimit(skeletonPose, ragdoll, legUpperRight, legLowerRight, new Vector3F(0, 0, -2.2f), new Vector3F(0, 0, 0.0f));
            AddTwistSwingLimit(ragdoll, legLowerRight, footRight, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.1f, -0.3f, -0.7f), new Vector3F(0.1f, 0.3f, 0.7f));
            #endregion

            #region ----- Add Motors -----

            ragdoll.Motors.AddRange(Enumerable.Repeat <RagdollMotor>(null, numberOfBones));
            ragdoll.Motors[pelvis]        = new RagdollMotor(pelvis, -1);
            ragdoll.Motors[backLower]     = new RagdollMotor(backLower, pelvis);
            ragdoll.Motors[backUpper]     = new RagdollMotor(backUpper, backLower);
            ragdoll.Motors[neck]          = new RagdollMotor(neck, backUpper);
            ragdoll.Motors[head]          = new RagdollMotor(head, neck);
            ragdoll.Motors[armUpperLeft]  = new RagdollMotor(armUpperLeft, backUpper);
            ragdoll.Motors[armLowerLeft]  = new RagdollMotor(armLowerLeft, armUpperLeft);
            ragdoll.Motors[handLeft]      = new RagdollMotor(handLeft, armLowerLeft);
            ragdoll.Motors[armUpperRight] = new RagdollMotor(armUpperRight, backUpper);
            ragdoll.Motors[armLowerRight] = new RagdollMotor(armLowerRight, armUpperRight);
            ragdoll.Motors[handRight]     = new RagdollMotor(handRight, armLowerRight);
            ragdoll.Motors[legUpperLeft]  = new RagdollMotor(legUpperLeft, pelvis);
            ragdoll.Motors[legLowerLeft]  = new RagdollMotor(legLowerLeft, legUpperLeft);
            ragdoll.Motors[footLeft]      = new RagdollMotor(footLeft, legLowerLeft);
            ragdoll.Motors[legUpperRight] = new RagdollMotor(legUpperRight, pelvis);
            ragdoll.Motors[legLowerRight] = new RagdollMotor(legLowerRight, legUpperRight);
            ragdoll.Motors[footRight]     = new RagdollMotor(footRight, legLowerRight);
            #endregion
        }
        //--------------------------------------------------------------
        #region Methods
        //--------------------------------------------------------------

        protected override void OnLoad()
        {
            // Add rigid bodies to simulation.
            var simulation = _services.GetInstance <Simulation>();

            // We use a random number generator with a custom seed.
            RandomHelper.Random = new Random(123);

            // ----- Add a ground plane.
            AddBody(simulation, "GroundPlane", Pose.Identity, new PlaneShape(Vector3F.UnitY, 0), MotionType.Static);

            // ----- Create a small flying sphere.
            AddBody(simulation, "Sphere", new Pose(new Vector3F(0, 1f, 0)), new SphereShape(0.2f), MotionType.Static);

            // ----- Create small walls at the level boundary.
            AddBody(simulation, "WallLeft", new Pose(new Vector3F(-30, 1, 0)), new BoxShape(0.3f, 2, 60), MotionType.Static);
            AddBody(simulation, "WallRight", new Pose(new Vector3F(30, 1, 0)), new BoxShape(0.3f, 2, 60), MotionType.Static);
            AddBody(simulation, "WallFront", new Pose(new Vector3F(0, 1, -30)), new BoxShape(60, 2, 0.3f), MotionType.Static);
            AddBody(simulation, "WallBack", new Pose(new Vector3F(0, 1, 30)), new BoxShape(60, 2, 0.3f), MotionType.Static);

            // ----- Create a few bigger objects.
            // We position the boxes so that we have a few corners we can run into. Character controllers
            // should be stable when the user runs into corners.
            AddBody(simulation, "House0", new Pose(new Vector3F(10, 1, -10)), new BoxShape(8, 2, 8f), MotionType.Static);
            AddBody(simulation, "House1", new Pose(new Vector3F(13, 1, -4)), new BoxShape(2, 2, 4), MotionType.Static);
            AddBody(simulation, "House2", new Pose(new Vector3F(10, 2, -15), Matrix33F.CreateRotationY(-0.3f)), new BoxShape(8, 4, 2), MotionType.Static);

            // ----- Create stairs with increasing step height.
            // Each step is a box. With this object we can test if our character can climb up
            // stairs. The character controller has a step height limit. Increasing step heights
            // let us test if the step height limit works.
            float       startHeight = 0;
            const float stepDepth   = 1f;

            for (int i = 0; i < 10; i++)
            {
                float    stepHeight = 0.1f + i * 0.05f;
                Vector3F position   = new Vector3F(0, startHeight + stepHeight / 2, -2 - i * stepDepth);
                AddBody(simulation, "Step" + i, new Pose(position), new BoxShape(2, stepHeight, stepDepth), MotionType.Static);

                startHeight += stepHeight;
            }

            // ----- V obstacle to test if we get stuck.
            AddBody(simulation, "V0", new Pose(new Vector3F(-5.5f, 0, 10), QuaternionF.CreateRotationZ(0.2f)), new BoxShape(1f, 2f, 2), MotionType.Static);
            AddBody(simulation, "V1", new Pose(new Vector3F(-4, 0, 10), QuaternionF.CreateRotationZ(-0.2f)), new BoxShape(1f, 2f, 2), MotionType.Static);

            // ----- Create a height field.
            // Terrain that is uneven is best modeled with a height field. Height fields are faster
            // than general triangle meshes.
            // The height direction is the y direction.
            // The height field lies in the x/z plane.
            var numberOfSamplesX = 20;
            var numberOfSamplesZ = 20;
            var samples          = new float[numberOfSamplesX * numberOfSamplesZ];

            // Create arbitrary height values.
            for (int z = 0; z < numberOfSamplesZ; z++)
            {
                for (int x = 0; x < numberOfSamplesX; x++)
                {
                    if (x == 0 || z == 0 || x == 19 || z == 19)
                    {
                        // Set this boundary elements to a low height, so that the height field is connected
                        // to the ground.
                        samples[z * numberOfSamplesX + x] = -1;
                    }
                    else
                    {
                        // A sine/cosine function that creates some interesting waves.
                        samples[z * numberOfSamplesX + x] = 1.0f + (float)(Math.Cos(z / 2f) * Math.Sin(x / 2f) * 1.0f);
                    }
                }
            }
            var heightField = new HeightField(0, 0, 20, 20, samples, numberOfSamplesX, numberOfSamplesZ);

            AddBody(simulation, "HeightField", new Pose(new Vector3F(10, 0, 10)), heightField, MotionType.Static);

            // ----- Create rubble on the floor (small random objects on the floor).
            // Our character should be able to move over small bumps on the ground.
            for (int i = 0; i < 50; i++)
            {
                Vector3F    position    = new Vector3F(RandomHelper.Random.NextFloat(-5, 5), 0, RandomHelper.Random.NextFloat(10, 20));
                QuaternionF orientation = RandomHelper.Random.NextQuaternionF();
                Vector3F    size        = RandomHelper.Random.NextVector3F(0.05f, 0.8f);
                AddBody(simulation, "Stone" + i, new Pose(position, orientation), new BoxShape(size), MotionType.Static);
            }

            // ----- Create some slopes to see how our character performs on/under sloped surfaces.
            // Here we can test how the character controller behaves if the head touches an inclined
            // ceiling.
            AddBody(simulation, "SlopeGround", new Pose(new Vector3F(-2, 1.8f, -12), QuaternionF.CreateRotationX(0.4f)), new BoxShape(2, 0.5f, 10), MotionType.Static);
            AddBody(simulation, "SlopeRoof", new Pose(new Vector3F(-2, 5.6f, -12), QuaternionF.CreateRotationX(-0.4f)), new BoxShape(2, 0.5f, 10), MotionType.Static);

            // Create slopes with increasing tilt angles.
            // The character controller has a slope limit. Only flat slopes should be climbable.
            // Movement between slopes should be smooth.
            Vector3F slopePosition = new Vector3F(-17, -0.25f, 6);
            BoxShape slopeShape    = new BoxShape(8, 0.5f, 5);

            for (int i = 1; i < 8; i++)
            {
                Matrix33F oldRotation = Matrix33F.CreateRotationX((i - 1) * MathHelper.ToRadians(10));
                Matrix33F rotation    = Matrix33F.CreateRotationX(i * MathHelper.ToRadians(10));

                slopePosition += (oldRotation * new Vector3F(0, 0, -slopeShape.WidthZ)) / 2
                                 + (rotation * new Vector3F(0, 0, -slopeShape.WidthZ)) / 2;

                AddBody(simulation, "Slope" + i, new Pose(slopePosition, rotation), slopeShape, MotionType.Static);
            }

            // Create slopes with decreasing tilt angles.
            slopePosition = new Vector3F(-8, -2, 5);
            slopeShape    = new BoxShape(8f, 0.5f, 5);
            for (int i = 1; i < 8; i++)
            {
                Matrix33F oldRotation = Matrix33F.CreateRotationX(MathHelper.ToRadians(40) - (i - 1) * MathHelper.ToRadians(10));
                Matrix33F rotation    = Matrix33F.CreateRotationX(MathHelper.ToRadians(40) - i * MathHelper.ToRadians(10));

                slopePosition += (oldRotation * new Vector3F(0, 0, -slopeShape.WidthZ)) / 2
                                 + (rotation * new Vector3F(0, 0, -slopeShape.WidthZ)) / 2;
                Vector3F position = slopePosition - rotation * new Vector3F(0, slopeShape.WidthY / 2, 0);

                AddBody(simulation, "Slope2" + i, new Pose(position, rotation), slopeShape, MotionType.Static);
            }

            // ----- Create a slope with a wall on one side.
            // This objects let's us test how the character controller behaves while falling and
            // sliding along a vertical wall. (Run up the slope and then jump down while moving into
            // the wall.)
            AddBody(simulation, "LongSlope", new Pose(new Vector3F(-24, 3, -10), Matrix33F.CreateRotationX(0.4f)), new BoxShape(4, 5f, 30), MotionType.Static);
            AddBody(simulation, "LongSlopeWall", new Pose(new Vector3F(-26, 5, -10)), new BoxShape(0.5f, 10f, 25), MotionType.Static);

            // ----- Create a trigger object that represents a ladder.
            var ladder = AddBody(simulation, "Ladder", new Pose(new Vector3F(-25.7f, 5, 0)), new BoxShape(0.5f, 10f, 1), MotionType.Static);

            ladder.CollisionObject.Type = CollisionObjectType.Trigger;

            // ----- Create a mesh object to test walking on triangle meshes.
            // Normally, the mesh would be loaded from a file. Here, we make a composite shape and
            // let DigitalRune Geometry compute a mesh for it. Then we throw away the composite
            // shape and use only the mesh. (We do this to test triangle meshes. Using the composite
            // shape instead of the triangle mesh would be a lot faster.)
            CompositeShape compositeShape = new CompositeShape();

            compositeShape.Children.Add(new GeometricObject(heightField, Pose.Identity));
            compositeShape.Children.Add(new GeometricObject(new CylinderShape(1, 2), new Pose(new Vector3F(10, 1, 10))));
            compositeShape.Children.Add(new GeometricObject(new SphereShape(3), new Pose(new Vector3F(15, 0, 15))));
            compositeShape.Children.Add(new GeometricObject(new BoxShape(4, 4, 3), new Pose(new Vector3F(15, 1.5f, 5))));
            compositeShape.Children.Add(new GeometricObject(new BoxShape(4, 4, 3), new Pose(new Vector3F(15, 1.5f, 0))));
            ITriangleMesh     mesh      = compositeShape.GetMesh(0.01f, 3);
            TriangleMeshShape meshShape = new TriangleMeshShape(mesh);

            // Collision detection speed for triangle meshes can be improved by using a spatial
            // partition. Here, we assign an AabbTree to the triangle mesh shape. The tree is
            // built automatically when needed and it stores triangle indices (therefore the generic
            // parameter of the AabbTree is int).
            meshShape.Partition = new AabbTree <int>()
            {
                // The tree is automatically built using a mixed top-down/bottom-up approach. Bottom-up
                // building is slower but produces better trees. If the tree building takes too long,
                // we can lower the BottomUpBuildThreshold (default is 128).
                BottomUpBuildThreshold = 0,
            };

            // Contact welding creates smoother contact normals - but it costs a bit of performance.
            meshShape.EnableContactWelding = true;

            AddBody(simulation, "Mesh", new Pose(new Vector3F(-30, 0, 10)), meshShape, MotionType.Static);

            // ----- Create a seesaw.
            var seesawBase = AddBody(simulation, "SeesawBase", new Pose(new Vector3F(5, 0.5f, 0)), new BoxShape(0.2f, 1, 1), MotionType.Static);
            var seesaw     = AddBody(simulation, "Seesaw", new Pose(new Vector3F(5, 1.05f, 0)), new BoxShape(5, 0.1f, 1), MotionType.Dynamic);

            // Attach the seesaw to the base using a hinge joint.
            simulation.Constraints.Add(new HingeJoint
            {
                BodyA            = seesaw,
                BodyB            = seesawBase,
                AnchorPoseALocal = new Pose(new Vector3F(0, 0, 0),
                                            new Matrix33F(0, 0, -1,
                                                          0, 1, 0,
                                                          1, 0, 0)),
                AnchorPoseBLocal = new Pose(new Vector3F(0, 0.5f, 0),
                                            new Matrix33F(0, 0, -1,
                                                          0, 1, 0,
                                                          1, 0, 0)),
                CollisionEnabled = false,
            });

            // ----- A platform that is moving up/down.
            _elevator = AddBody(simulation, "Elevator", new Pose(new Vector3F(5, -1f, 5)), new BoxShape(3, 1f, 3), MotionType.Kinematic);
            _elevator.LinearVelocity = new Vector3F(2, 2, 0);

            // ----- A platform that is moving sideways.
            _pusher = AddBody(simulation, "Pusher", new Pose(new Vector3F(15, 0.5f, 0)), new BoxShape(3, 1f, 3), MotionType.Kinematic);
            _pusher.LinearVelocity = new Vector3F(0, 0, 2);

            // ----- Create conveyor belt with two static boxes on the sides.
            AddBody(simulation, "ConveyorSide0", new Pose(new Vector3F(19, 0.25f, 0)), new BoxShape(0.8f, 0.5f, 8f), MotionType.Static);
            AddBody(simulation, "ConveyorSide1", new Pose(new Vector3F(21, 0.25f, 0)), new BoxShape(0.8f, 0.5f, 8f), MotionType.Static);

            // The conveyor belt is a simple box with a special material.
            var             conveyorBelt = AddBody(simulation, "ConveyorBelt", new Pose(new Vector3F(20, 0.25f, 0)), new BoxShape(1f, 0.51f, 8f), MotionType.Static);
            UniformMaterial materialWithSurfaceMotion = new UniformMaterial("ConveyorBelt", true) // Important: The second parameter enables the surface
            {                                                                                     // motion. It has to be set to true in the constructor!
                SurfaceMotion = new Vector3F(0, 0, 1),                                            // The surface motion relative to the object.
            };

            conveyorBelt.Material = materialWithSurfaceMotion;

            // ----- Distribute a few dynamic spheres and boxes across the landscape.
            SphereShape sphereShape = new SphereShape(0.5f);

            for (int i = 0; i < 10; i++)
            {
                Vector3F position = RandomHelper.Random.NextVector3F(-15, 15);
                position.Y = 20;

                AddBody(simulation, "Sphere" + i, new Pose(position), sphereShape, MotionType.Dynamic);
            }

            BoxShape boxShape = new BoxShape(1, 1, 1);

            for (int i = 0; i < 10; i++)
            {
                Vector3F position = RandomHelper.Random.NextVector3F(-15, 15);
                position.Y = 20;

                AddBody(simulation, "Box" + i, new Pose(position), boxShape, MotionType.Dynamic);
            }
        }
示例#9
0
        /// <summary>
        /// Creates a <see cref="Ragdoll"/> for an Xbox LIVE Avatar. (Only available on Xbox 360.)
        /// </summary>
        /// <param name="skeleton">The skeleton of the Xbox LIVE Avatar.</param>
        /// <param name="simulation">The simulation.</param>
        /// <returns>The avatar ragdoll.</returns>
        /// <exception cref="ArgumentNullException">
        /// <paramref name="skeleton"/> or <paramref name="simulation"/> is
        /// <see langword="null"/>.
        /// </exception>
        /// <remarks>
        /// This method is available only in the Xbox 360 build of the
        /// DigitalRune.Physics.Specialized.dll.
        /// </remarks>
        public static Ragdoll CreateAvatarRagdoll(Skeleton skeleton, Simulation simulation)
        {
            if (skeleton == null)
            {
                throw new ArgumentNullException("skeleton");
            }
            if (simulation == null)
            {
                throw new ArgumentNullException("simulation");
            }

            var ragdoll = new Ragdoll();

            // The lists ragdoll.Bodies, ragdoll.BodyOffsets and _motors contain one entry per bone - even if there
            // is no RigidBody for this bone. - This wastes memory but simplifies the code.
            for (int i = 0; i < AvatarRenderer.BoneCount; i++)
            {
                ragdoll.Bodies.Add(null);
                ragdoll.BodyOffsets.Add(Pose.Identity);
                ragdoll.Joints.Add(null);
                ragdoll.Limits.Add(null);
                ragdoll.Motors.Add(null);
            }

            // ----- Create bodies.
            // We use the same mass for all bodies. This is not physically correct but it makes the
            // simulation more stable, for several reasons:
            // - It is better to avoid large mass differences. Therefore, all limbs have the same mass.
            // - Capsule shapes have a low inertia value about their height axis. This causes instability
            //   and it is better to use larger inertia values.
            var massFrame = MassFrame.FromShapeAndMass(new SphereShape(0.2f), Vector3F.One, 4, 0.1f, 1);

            // Use standard material.
            var material = new UniformMaterial();

            // Create rigid bodies for the important bones. The shapes have been manually adapted to
            // produce useful results for thin and overweight avatars.
            // Without offset, the bodies are centered at the joint. ragdoll.BodyOffsets stores an offset pose
            // for each body. Instead, we could use TransformedShape but we can easily handle that
            // ourselves.
            // The collar bones are special, they use dummy shapes and are only used to connect the
            // shoulder bones.
            ragdoll.Bodies[(int)AvatarBone.Root]               = new RigidBody(new BoxShape(0.22f, 0.16f, 0.16f), massFrame, material);
            ragdoll.BodyOffsets[(int)AvatarBone.Root]          = new Pose(new Vector3F(0, -0.08f, -0.01f), QuaternionF.CreateRotationX(-0.0f));
            ragdoll.Bodies[(int)AvatarBone.BackLower]          = new RigidBody(new BoxShape(0.22f, 0.16f, 0.16f), massFrame, material);
            ragdoll.BodyOffsets[(int)AvatarBone.BackLower]     = new Pose(new Vector3F(0, 0.08f, -0.01f), QuaternionF.CreateRotationX(-0.0f));
            ragdoll.Bodies[(int)AvatarBone.BackUpper]          = new RigidBody(new BoxShape(0.22f, 0.16f, 0.16f), massFrame, material);
            ragdoll.BodyOffsets[(int)AvatarBone.BackUpper]     = new Pose(new Vector3F(0, 0.08f, -0.01f), QuaternionF.CreateRotationX(-0.1f));
            ragdoll.Bodies[(int)AvatarBone.Neck]               = new RigidBody(new CapsuleShape(0.04f, 0.09f), massFrame, material);
            ragdoll.Bodies[(int)AvatarBone.Head]               = new RigidBody(new SphereShape(0.15f), massFrame, material);
            ragdoll.BodyOffsets[(int)AvatarBone.Head]          = new Pose(new Vector3F(0, 0.1f, 0));
            ragdoll.Bodies[(int)AvatarBone.CollarLeft]         = new RigidBody(Shape.Empty, massFrame, material);
            ragdoll.Bodies[(int)AvatarBone.CollarRight]        = new RigidBody(Shape.Empty, massFrame, material);
            ragdoll.Bodies[(int)AvatarBone.ShoulderLeft]       = new RigidBody(new CapsuleShape(0.04f, 0.25f), massFrame, material);
            ragdoll.BodyOffsets[(int)AvatarBone.ShoulderLeft]  = new Pose(new Vector3F(0.08f, 0, -0.02f), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
            ragdoll.Bodies[(int)AvatarBone.ShoulderRight]      = new RigidBody(new CapsuleShape(0.04f, 0.25f), massFrame, material);
            ragdoll.BodyOffsets[(int)AvatarBone.ShoulderRight] = new Pose(new Vector3F(-0.08f, 0, -0.02f), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
            ragdoll.Bodies[(int)AvatarBone.ElbowLeft]          = new RigidBody(new CapsuleShape(0.04f, 0.21f), massFrame, material);
            ragdoll.BodyOffsets[(int)AvatarBone.ElbowLeft]     = new Pose(new Vector3F(0.06f, 0, -0.02f), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
            ragdoll.Bodies[(int)AvatarBone.ElbowRight]         = new RigidBody(new CapsuleShape(0.04f, 0.21f), massFrame, material);
            ragdoll.BodyOffsets[(int)AvatarBone.ElbowRight]    = new Pose(new Vector3F(-0.06f, 0, -0.02f), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
            ragdoll.Bodies[(int)AvatarBone.WristLeft]          = new RigidBody(new BoxShape(0.1f, 0.04f, 0.1f), massFrame, material);
            ragdoll.BodyOffsets[(int)AvatarBone.WristLeft]     = new Pose(new Vector3F(0.06f, -0.02f, -0.01f), QuaternionF.CreateRotationZ(0.0f));
            ragdoll.Bodies[(int)AvatarBone.WristRight]         = new RigidBody(new BoxShape(0.1f, 0.04f, 0.1f), massFrame, material);
            ragdoll.BodyOffsets[(int)AvatarBone.WristRight]    = new Pose(new Vector3F(-0.06f, -0.02f, -0.01f), QuaternionF.CreateRotationZ(0.0f));
            ragdoll.Bodies[(int)AvatarBone.HipLeft]            = new RigidBody(new CapsuleShape(0.06f, 0.34f), massFrame, material);
            ragdoll.BodyOffsets[(int)AvatarBone.HipLeft]       = new Pose(new Vector3F(0, -0.14f, -0.02f), QuaternionF.CreateRotationX(0.1f));
            ragdoll.Bodies[(int)AvatarBone.HipRight]           = new RigidBody(new CapsuleShape(0.06f, 0.34f), massFrame, material);
            ragdoll.BodyOffsets[(int)AvatarBone.HipRight]      = new Pose(new Vector3F(0, -0.14f, -0.02f), QuaternionF.CreateRotationX(0.1f));
            ragdoll.Bodies[(int)AvatarBone.KneeLeft]           = new RigidBody(new CapsuleShape(0.06f, 0.36f), massFrame, material);
            ragdoll.BodyOffsets[(int)AvatarBone.KneeLeft]      = new Pose(new Vector3F(0, -0.18f, -0.04f), QuaternionF.CreateRotationX(0.1f));
            ragdoll.Bodies[(int)AvatarBone.KneeRight]          = new RigidBody(new CapsuleShape(0.06f, 0.36f), massFrame, material);
            ragdoll.BodyOffsets[(int)AvatarBone.KneeRight]     = new Pose(new Vector3F(0, -0.18f, -0.04f), QuaternionF.CreateRotationX(0.1f));
            ragdoll.Bodies[(int)AvatarBone.AnkleLeft]          = new RigidBody(new BoxShape(0.1f, 0.06f, 0.22f), massFrame, material);
            ragdoll.BodyOffsets[(int)AvatarBone.AnkleLeft]     = new Pose(new Vector3F(0, -0.07f, 0.05f), QuaternionF.CreateRotationZ(0));
            ragdoll.Bodies[(int)AvatarBone.AnkleRight]         = new RigidBody(new BoxShape(0.1f, 0.06f, 0.22f), massFrame, material);
            ragdoll.BodyOffsets[(int)AvatarBone.AnkleRight]    = new Pose(new Vector3F(0, -0.07f, 0.05f), QuaternionF.CreateRotationZ(0));

            // ----- Add joint constraints.
            const float jointErrorReduction = 0.2f;
            const float jointSoftness       = 0.0001f;

            AddJoint(ragdoll, skeleton, AvatarBone.Root, AvatarBone.BackLower, jointErrorReduction, jointSoftness);
            AddJoint(ragdoll, skeleton, AvatarBone.BackLower, AvatarBone.BackUpper, jointErrorReduction, jointSoftness);
            AddJoint(ragdoll, skeleton, AvatarBone.BackUpper, AvatarBone.Neck, 0.6f, 0.000001f);
            AddJoint(ragdoll, skeleton, AvatarBone.Neck, AvatarBone.Head, 0.6f, 0.000001f);
            AddJoint(ragdoll, skeleton, AvatarBone.BackUpper, AvatarBone.CollarLeft, jointErrorReduction, jointSoftness);
            AddJoint(ragdoll, skeleton, AvatarBone.BackUpper, AvatarBone.CollarRight, jointErrorReduction, jointSoftness);
            AddJoint(ragdoll, skeleton, AvatarBone.CollarLeft, AvatarBone.ShoulderLeft, jointErrorReduction, jointSoftness);
            AddJoint(ragdoll, skeleton, AvatarBone.CollarRight, AvatarBone.ShoulderRight, jointErrorReduction, jointSoftness);
            AddJoint(ragdoll, skeleton, AvatarBone.ShoulderLeft, AvatarBone.ElbowLeft, jointErrorReduction, jointSoftness);
            AddJoint(ragdoll, skeleton, AvatarBone.ShoulderRight, AvatarBone.ElbowRight, jointErrorReduction, jointSoftness);
            AddJoint(ragdoll, skeleton, AvatarBone.ElbowLeft, AvatarBone.WristLeft, jointErrorReduction, jointSoftness);
            AddJoint(ragdoll, skeleton, AvatarBone.ElbowRight, AvatarBone.WristRight, jointErrorReduction, jointSoftness);
            AddJoint(ragdoll, skeleton, AvatarBone.Root, AvatarBone.HipLeft, jointErrorReduction, jointSoftness);
            AddJoint(ragdoll, skeleton, AvatarBone.Root, AvatarBone.HipRight, jointErrorReduction, jointSoftness);
            AddJoint(ragdoll, skeleton, AvatarBone.HipLeft, AvatarBone.KneeLeft, jointErrorReduction, jointSoftness);
            AddJoint(ragdoll, skeleton, AvatarBone.HipRight, AvatarBone.KneeRight, jointErrorReduction, jointSoftness);
            AddJoint(ragdoll, skeleton, AvatarBone.KneeLeft, AvatarBone.AnkleLeft, jointErrorReduction, jointSoftness);
            AddJoint(ragdoll, skeleton, AvatarBone.KneeRight, AvatarBone.AnkleRight, jointErrorReduction, jointSoftness);

            // ----- Add constraint limits.
            // We use TwistSwingLimits to define an allowed twist and swing cone for the joints.
            // Exceptions are the back and knees, where we use AngularLimits to create hinges.
            // (We could also create a hinge with a TwistSwingLimit where the twist axis is the hinge
            // axis and no swing is allowed - but AngularLimits create more stable hinges.)
            // Another exception are the collar bones joint. We use AngularLimits to disallow any
            // rotations.
            AddAngularLimit(ragdoll, skeleton, AvatarBone.Root, AvatarBone.BackLower,
                            skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.BackLower).Rotation.Conjugated.ToRotationMatrix33(),
                            skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.BackLower).Rotation.Conjugated.ToRotationMatrix33(),
                            new Vector3F(-0.3f, 0, 0), new Vector3F(0.3f, 0, 0));

            AddAngularLimit(ragdoll, skeleton, AvatarBone.BackLower, AvatarBone.BackUpper,
                            skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.BackUpper).Rotation.Conjugated.ToRotationMatrix33(),
                            skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.BackUpper).Rotation.Conjugated.ToRotationMatrix33(),
                            new Vector3F(-0.3f, 0, 0), new Vector3F(0.4f, 0, 0));

            var rotationZ90Degrees = Matrix33F.CreateRotationZ(ConstantsF.PiOver2);

            AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.BackUpper, AvatarBone.Neck, rotationZ90Degrees, rotationZ90Degrees, new Vector3F(-0.1f, -0.3f, -0.3f), new Vector3F(+0.1f, +0.3f, +0.3f));

            AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.Neck, AvatarBone.Head, rotationZ90Degrees, rotationZ90Degrees, new Vector3F(-0.1f, -0.6f, -0.6f), new Vector3F(+0.1f, +0.6f, +0.6f));

            AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.BackUpper, AvatarBone.CollarLeft,
                               skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.CollarLeft).Rotation.Conjugated.ToRotationMatrix33(),
                               skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.CollarLeft).Rotation.Conjugated.ToRotationMatrix33(),
                               new Vector3F(0), new Vector3F(0));

            AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.BackUpper, AvatarBone.CollarRight,
                               skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.CollarRight).Rotation.Conjugated.ToRotationMatrix33(),
                               skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.CollarRight).Rotation.Conjugated.ToRotationMatrix33(),
                               new Vector3F(0), new Vector3F(0));

            AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.CollarLeft, AvatarBone.ShoulderLeft, Matrix33F.Identity, Matrix33F.CreateRotationY(0.7f), new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(+0.7f, +1.2f, +1.2f));
            AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.CollarRight, AvatarBone.ShoulderRight, Matrix33F.Identity, Matrix33F.CreateRotationY(-0.7f), new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(+0.7f, +1.2f, +1.2f));
            AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.ShoulderLeft, AvatarBone.ElbowLeft, Matrix33F.Identity, Matrix33F.CreateRotationY(1.2f), new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(+0.7f, +1.2f, +1.2f));
            AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.ShoulderRight, AvatarBone.ElbowRight, Matrix33F.Identity, Matrix33F.CreateRotationY(-1.2f), new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(+0.7f, +1.2f, +1.2f));
            AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.ElbowLeft, AvatarBone.WristLeft, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.7f, -0.7f, -0.7f), new Vector3F(+0.7f, +0.7f, +0.7f));
            AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.ElbowRight, AvatarBone.WristRight, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.7f, -0.7f, -0.7f), new Vector3F(+0.7f, +0.7f, +0.7f));
            AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.Root, AvatarBone.HipLeft, rotationZ90Degrees, Matrix33F.CreateRotationX(-1.2f) * Matrix33F.CreateRotationZ(ConstantsF.PiOver2 + 0.2f), new Vector3F(-0.1f, -1.5f, -0.7f), new Vector3F(+0.1f, +1.5f, +0.7f));
            AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.Root, AvatarBone.HipRight, rotationZ90Degrees, Matrix33F.CreateRotationX(-1.2f) * Matrix33F.CreateRotationZ(ConstantsF.PiOver2 - 0.2f), new Vector3F(-0.1f, -1.5f, -0.7f), new Vector3F(+0.1f, +1.5f, +0.7f));

            AddAngularLimit(ragdoll, skeleton, AvatarBone.HipLeft, AvatarBone.KneeLeft,
                            skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.KneeLeft).Rotation.Conjugated.ToRotationMatrix33(),
                            skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.KneeLeft).Rotation.Conjugated.ToRotationMatrix33(),
                            new Vector3F(0, 0, 0), new Vector3F(2.2f, 0, 0));

            AddAngularLimit(ragdoll, skeleton, AvatarBone.HipRight, AvatarBone.KneeRight,
                            skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.KneeRight).Rotation.Conjugated.ToRotationMatrix33(),
                            skeleton.GetBindPoseAbsoluteInverse((int)AvatarBone.KneeRight).Rotation.Conjugated.ToRotationMatrix33(),
                            new Vector3F(0, 0, 0), new Vector3F(2.2f, 0, 0));

            AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.KneeLeft, AvatarBone.AnkleLeft, rotationZ90Degrees, rotationZ90Degrees, new Vector3F(-0.1f, -0.7f, -0.3f), new Vector3F(+0.1f, +0.7f, +0.3f));
            AddTwistSwingLimit(ragdoll, skeleton, AvatarBone.KneeRight, AvatarBone.AnkleRight, rotationZ90Degrees, rotationZ90Degrees, new Vector3F(-0.1f, -0.7f, -0.3f), new Vector3F(+0.1f, +0.7f, +0.3f));

            // ----- Add motors
            // We use QuaternionMotors to create forces that rotate the bones into desired poses.
            // This can be used for damping, spring or animating the ragdoll.
            AddMotor(ragdoll, (AvatarBone)(-1), AvatarBone.Root);
            AddMotor(ragdoll, AvatarBone.Root, AvatarBone.BackLower);
            AddMotor(ragdoll, AvatarBone.BackLower, AvatarBone.BackUpper);
            AddMotor(ragdoll, AvatarBone.BackUpper, AvatarBone.Neck);
            AddMotor(ragdoll, AvatarBone.Neck, AvatarBone.Head);
            AddMotor(ragdoll, AvatarBone.BackUpper, AvatarBone.CollarLeft);
            AddMotor(ragdoll, AvatarBone.BackUpper, AvatarBone.CollarRight);
            AddMotor(ragdoll, AvatarBone.CollarLeft, AvatarBone.ShoulderLeft);
            AddMotor(ragdoll, AvatarBone.CollarRight, AvatarBone.ShoulderRight);
            AddMotor(ragdoll, AvatarBone.ShoulderLeft, AvatarBone.ElbowLeft);
            AddMotor(ragdoll, AvatarBone.ShoulderRight, AvatarBone.ElbowRight);
            AddMotor(ragdoll, AvatarBone.ElbowLeft, AvatarBone.WristLeft);
            AddMotor(ragdoll, AvatarBone.ElbowRight, AvatarBone.WristRight);
            AddMotor(ragdoll, AvatarBone.Root, AvatarBone.HipLeft);
            AddMotor(ragdoll, AvatarBone.Root, AvatarBone.HipRight);
            AddMotor(ragdoll, AvatarBone.HipLeft, AvatarBone.KneeLeft);
            AddMotor(ragdoll, AvatarBone.HipRight, AvatarBone.KneeRight);
            AddMotor(ragdoll, AvatarBone.KneeLeft, AvatarBone.AnkleLeft);
            AddMotor(ragdoll, AvatarBone.KneeRight, AvatarBone.AnkleRight);

            // ----- Set collision filters.
            // Collisions between connected bones have been disabled with Constraint.CollisionEnabled
            // = false in the joints. We need to disable a few other collisions.
            // Following bodies do not collide with anything. They are only used to connect other
            // bones.
            ragdoll.Bodies[(int)AvatarBone.Neck].CollisionObject.Enabled        = false;
            ragdoll.Bodies[(int)AvatarBone.CollarLeft].CollisionObject.Enabled  = false;
            ragdoll.Bodies[(int)AvatarBone.CollarRight].CollisionObject.Enabled = false;

            // We disable filters for following body pairs because they are usually penetrating each
            // other, which needs to be ignored.
            var filter = simulation.CollisionDomain.CollisionDetection.CollisionFilter as CollisionFilter;

            if (filter != null)
            {
                filter.Set(ragdoll.Bodies[(int)AvatarBone.BackUpper].CollisionObject, ragdoll.Bodies[(int)AvatarBone.ShoulderLeft].CollisionObject, false);
                filter.Set(ragdoll.Bodies[(int)AvatarBone.BackUpper].CollisionObject, ragdoll.Bodies[(int)AvatarBone.ShoulderRight].CollisionObject, false);
            }

            return(ragdoll);
        }
示例#10
0
        /// <summary>
        /// Called when a mesh should be generated for the shape.
        /// </summary>
        /// <param name="absoluteDistanceThreshold">The absolute distance threshold.</param>
        /// <param name="iterationLimit">The iteration limit.</param>
        /// <returns>The triangle mesh for this shape.</returns>
        protected override TriangleMesh OnGetMesh(float absoluteDistanceThreshold, int iterationLimit)
        {
            // Estimate required segment angle for given accuracy.
            // (Easy to derive from simple drawing of a circle segment with a triangle used to represent
            // the segment.)
            float alpha            = (float)Math.Acos((Radius - absoluteDistanceThreshold) / Radius) * 2;
            int   numberOfSegments = (int)Math.Ceiling(ConstantsF.PiOver2 / alpha) * 4;

            // Apply the iteration limit - in case absoluteDistanceThreshold is 0.
            // Lets say each iteration doubles the number of segments. This is an arbitrary interpretation
            // of the "iteration limit".
            numberOfSegments = Math.Min(numberOfSegments, 2 << iterationLimit);

            alpha = ConstantsF.TwoPi / numberOfSegments;

            TriangleMesh mesh = new TriangleMesh();

            // The world space vertices are created by rotating "radius vectors" with this rotations.
            QuaternionF rotationY = QuaternionF.CreateRotationY(alpha);
            QuaternionF rotationZ = QuaternionF.CreateRotationZ(alpha);

            // We use two nested loops: In each loop a "radius vector" is rotated further to get a
            // new vertex.
            Vector3F rLow = Vector3F.UnitX * Radius; // Radius vector for the lower vertex.

            for (int i = 1; i <= numberOfSegments / 4; i++)
            {
                Vector3F rHigh = rotationZ.Rotate(rLow); // Radius vector for the higher vertex.

                // In the inner loop we create lines and triangles between 4 vertices, which are created
                // with the radius vectors rLow0, rLow1, rHigh0, rHigh1.
                Vector3F rLow0  = rLow;
                Vector3F rHigh0 = rHigh;
                for (int j = 1; j <= numberOfSegments; j++)
                {
                    Vector3F rLow1  = rotationY.Rotate(rLow0);
                    Vector3F rHigh1 = rotationY.Rotate(rHigh0);

                    // Two top hemisphere triangles
                    mesh.Add(new Triangle
                    {
                        Vertex0 = new Vector3F(0, Height / 2 - Radius, 0) + rLow0,
                        Vertex1 = new Vector3F(0, Height / 2 - Radius, 0) + rLow1,
                        Vertex2 = new Vector3F(0, Height / 2 - Radius, 0) + rHigh0,
                    }, false);
                    if (i < numberOfSegments / 4) // At the "northpole" only a triangle is needed. No quad.
                    {
                        mesh.Add(new Triangle
                        {
                            Vertex0 = new Vector3F(0, Height / 2 - Radius, 0) + rLow1,
                            Vertex1 = new Vector3F(0, Height / 2 - Radius, 0) + rHigh1,
                            Vertex2 = new Vector3F(0, Height / 2 - Radius, 0) + rHigh0,
                        }, false);
                    }

                    // Two bottom hemisphere triangles
                    mesh.Add(new Triangle
                    {
                        Vertex0 = new Vector3F(0, -Height / 2 + Radius, 0) - rLow0,
                        Vertex1 = new Vector3F(0, -Height / 2 + Radius, 0) - rHigh0,
                        Vertex2 = new Vector3F(0, -Height / 2 + Radius, 0) - rLow1,
                    }, false);
                    if (i < numberOfSegments / 4) // At the "southpole" only a triangle is needed. No quad.
                    {
                        mesh.Add(new Triangle
                        {
                            Vertex0 = new Vector3F(0, -Height / 2 + Radius, 0) - rLow1,
                            Vertex1 = new Vector3F(0, -Height / 2 + Radius, 0) - rHigh0,
                            Vertex2 = new Vector3F(0, -Height / 2 + Radius, 0) - rHigh1,
                        }, false);
                    }

                    // Two side triangles
                    if (i == 1)
                    {
                        mesh.Add(new Triangle
                        {
                            Vertex0 = new Vector3F(0, -Height / 2 + Radius, 0) + rLow0,
                            Vertex1 = new Vector3F(0, -Height / 2 + Radius, 0) + rLow1,
                            Vertex2 = new Vector3F(0, Height / 2 - Radius, 0) + rLow0,
                        }, false);
                        mesh.Add(new Triangle
                        {
                            Vertex0 = new Vector3F(0, -Height / 2 + Radius, 0) + rLow1,
                            Vertex1 = new Vector3F(0, Height / 2 - Radius, 0) + rLow1,
                            Vertex2 = new Vector3F(0, Height / 2 - Radius, 0) + rLow0,
                        }, false);
                    }

                    rLow0  = rLow1;
                    rHigh0 = rHigh1;
                }

                rLow = rHigh;
            }

            mesh.WeldVertices();

            return(mesh);
        }
        public ConstraintSample3(Microsoft.Xna.Framework.Game game)
            : base(game)
        {
            // Add basic force effects.
            Simulation.ForceEffects.Add(new Gravity());
            Simulation.ForceEffects.Add(new Damping());

            // Add a ground plane.
            RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0))
            {
                Name       = "GroundPlane", // Names are not required but helpful for debugging.
                MotionType = MotionType.Static,
            };

            Simulation.RigidBodies.Add(groundPlane);

            // ----- HingeJoint with AngularVelocityMotor
            // A board is fixed on a pole like a wind wheel.
            // An AngularVelocityMotor creates a rotation with constant angular velocity around the
            // hinge axis.
            RigidBody box0 = new RigidBody(new BoxShape(0.1f, 2f, 0.1f))
            {
                Pose       = new Pose(new Vector3F(-2, 1, 0)),
                MotionType = MotionType.Static,
            };

            Simulation.RigidBodies.Add(box0);
            RigidBody box1 = new RigidBody(new BoxShape(0.1f, 0.4f, 1f))
            {
                Pose = new Pose(new Vector3F(-2 + 0.05f, 1.8f, 0))
            };

            Simulation.RigidBodies.Add(box1);
            HingeJoint hingeJoint = new HingeJoint
            {
                BodyA            = box0,
                BodyB            = box1,
                AnchorPoseALocal = new Pose(new Vector3F(0.05f, 0.8f, 0)),
                AnchorPoseBLocal = new Pose(new Vector3F(-0.05f, 0, 0)),
                CollisionEnabled = false,
            };

            Simulation.Constraints.Add(hingeJoint);
            AngularVelocityMotor angularVelocityMotor = new AngularVelocityMotor
            {
                BodyA = box0,
                // The rotation axis is the local x axis of BodyA.
                AxisALocal     = Vector3F.UnitX,
                BodyB          = box1,
                TargetVelocity = 10,
                // The motor power is limit, so that the rotation can be stopped by other objects blocking
                // the movement.
                MaxForce = 10000,
                // The HingeJoint controls all other axes. So this motor must only act on the hinge
                // axis.
                UseSingleAxisMode = true,
                CollisionEnabled  = false,
            };

            Simulation.Constraints.Add(angularVelocityMotor);

            // ----- A PrismaticJoint with a LinearVelocityMotor.
            RigidBody box2 = new RigidBody(new BoxShape(0.7f, 0.7f, 0.7f))
            {
                Pose       = new Pose(new Vector3F(0, 2, 0)),
                MotionType = MotionType.Static,
            };

            Simulation.RigidBodies.Add(box2);
            RigidBody box3 = new RigidBody(new BoxShape(0.5f, 1.5f, 0.5f))
            {
                Pose = new Pose(new Vector3F(0, 1, 0))
            };

            Simulation.RigidBodies.Add(box3);
            _prismaticJoint = new PrismaticJoint
            {
                BodyA            = box2,
                BodyB            = box3,
                AnchorPoseALocal = new Pose(new Vector3F(0, 0, 0), new Matrix33F(0, 1, 0,
                                                                                 -1, 0, 0,
                                                                                 0, 0, 1)),
                AnchorPoseBLocal = new Pose(new Vector3F(0, 0, 0), new Matrix33F(0, 1, 0,
                                                                                 -1, 0, 0,
                                                                                 0, 0, 1)),
                CollisionEnabled = false,
            };
            Simulation.Constraints.Add(_prismaticJoint);
            _linearVelocityMotor = new LinearVelocityMotor
            {
                BodyA             = box2,
                AxisALocal        = -Vector3F.UnitY,
                BodyB             = box3,
                TargetVelocity    = 1,
                CollisionEnabled  = false,
                MaxForce          = 10000,
                UseSingleAxisMode = true,
            };
            Simulation.Constraints.Add(_linearVelocityMotor);

            // ----- A BallJoint with a TwistSwingLimit and a QuaternionMotor
            // The ball joint connects a cylinder to a static box.
            // The twist-swing limits rotational movement.
            // The quaternion motor acts like a spring that controls the angle of joint.
            RigidBody box4 = new RigidBody(new BoxShape(0.5f, 0.5f, 0.5f))
            {
                Pose       = new Pose(new Vector3F(2, 2, 0)),
                MotionType = MotionType.Static,
            };

            Simulation.RigidBodies.Add(box4);
            RigidBody cylinder0 = new RigidBody(new CylinderShape(0.1f, 0.75f))
            {
                Pose = new Pose(new Vector3F(2, 2 - 0.75f / 2 - 0.25f, 0))
            };

            Simulation.RigidBodies.Add(cylinder0);
            _ballJoint = new BallJoint
            {
                BodyA = box4,
                BodyB = cylinder0,
                AnchorPositionALocal = new Vector3F(0, -0.25f, 0),
                AnchorPositionBLocal = new Vector3F(0, 0.75f / 2, 0),
                CollisionEnabled     = false,
            };
            Simulation.Constraints.Add(_ballJoint);
            _twistSwingLimit = new TwistSwingLimit
            {
                BodyA = box4,
                // The first column is the twist axis (-y). The other two columns are the swing axes.
                AnchorOrientationALocal = new Matrix33F(0, 1, 0,
                                                        -1, 0, 0,
                                                        0, 0, 1),
                BodyB = cylinder0,
                AnchorOrientationBLocal = new Matrix33F(0, 1, 0,
                                                        -1, 0, 0,
                                                        0, 0, 1),
                CollisionEnabled = false,
                // The twist is limited to +/- 10°. The swing limits are +/- 40° and +/- 60°. This creates
                // a deformed cone that limits the swing movements (see visualization).
                Minimum = new Vector3F(-MathHelper.ToRadians(10), -MathHelper.ToRadians(40), -MathHelper.ToRadians(60)),
                Maximum = new Vector3F(MathHelper.ToRadians(10), MathHelper.ToRadians(40), MathHelper.ToRadians(60)),
            };
            Simulation.Constraints.Add(_twistSwingLimit);
            QuaternionMotor quaternionMotor = new QuaternionMotor
            {
                BodyA = box4,
                AnchorOrientationALocal = Matrix33F.Identity,
                BodyB = cylinder0,
                AnchorOrientationBLocal = Matrix33F.Identity,
                CollisionEnabled        = false,
                // The QuaternionMotor controls the orientation of the second body relative to the first
                // body. Here, we define that the cylinder should swing 30° away from the default
                // orientation.
                TargetOrientation = QuaternionF.CreateRotationZ(MathHelper.ToRadians(30)),
                // Position and orientation motors are similar to damped-springs. We can control
                // the stiffness and damping of the spring. (It is also possible to set the SpringConstant
                // to 0 if the QuaternionMotor should only act as a rotational damping.)
                SpringConstant  = 100,
                DampingConstant = 20,
            };

            Simulation.Constraints.Add(quaternionMotor);
        }
示例#12
0
        // Creates a lot of random objects.
        private void CreateRandomObjects()
        {
            var random = new Random();

            var isFirstHeightField = true;

            int currentShape    = 0;
            int numberOfObjects = 0;

            while (true)
            {
                numberOfObjects++;
                if (numberOfObjects > ObjectsPerType)
                {
                    currentShape++;
                    numberOfObjects = 0;
                }

                Shape shape;
                switch (currentShape)
                {
                case 0:
                    // Box
                    shape = new BoxShape(ObjectSize, ObjectSize * 2, ObjectSize * 3);
                    break;

                case 1:
                    // Capsule
                    shape = new CapsuleShape(0.3f * ObjectSize, 2 * ObjectSize);
                    break;

                case 2:
                    // Cone
                    shape = new ConeShape(1 * ObjectSize, 2 * ObjectSize);
                    break;

                case 3:
                    // Cylinder
                    shape = new CylinderShape(0.4f * ObjectSize, 2 * ObjectSize);
                    break;

                case 4:
                    // Sphere
                    shape = new SphereShape(ObjectSize);
                    break;

                case 5:
                    // Convex hull of several points.
                    ConvexHullOfPoints hull = new ConvexHullOfPoints();
                    hull.Points.Add(new Vector3F(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize));
                    hull.Points.Add(new Vector3F(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize));
                    hull.Points.Add(new Vector3F(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
                    hull.Points.Add(new Vector3F(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
                    hull.Points.Add(new Vector3F(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize));
                    shape = hull;
                    break;

                case 6:
                    // A composite shape: two boxes that form a "T" shape.
                    var composite = new CompositeShape();
                    composite.Children.Add(
                        new GeometricObject(
                            new BoxShape(ObjectSize, 3 * ObjectSize, ObjectSize),
                            new Pose(new Vector3F(0, 0, 0))));
                    composite.Children.Add(
                        new GeometricObject(
                            new BoxShape(2 * ObjectSize, ObjectSize, ObjectSize),
                            new Pose(new Vector3F(0, 2 * ObjectSize, 0))));
                    shape = composite;
                    break;

                case 7:
                    shape = new CircleShape(ObjectSize);
                    break;

                case 8:
                {
                    var compBvh = new CompositeShape();
                    compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3F(0, 0.5f, 0), Matrix33F.Identity)));
                    compBvh.Children.Add(new GeometricObject(new BoxShape(0.8f, 0.5f, 0.5f), new Pose(new Vector3F(0.5f, 0.7f, 0), Matrix33F.CreateRotationZ(-MathHelper.ToRadians(15)))));
                    compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3F(0, 1.15f, 0), Matrix33F.Identity)));
                    compBvh.Children.Add(new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3F(0.6f, 1.15f, 0), Matrix33F.CreateRotationX(0.3f))));
                    compBvh.Partition = new AabbTree <int>();
                    shape             = compBvh;
                    break;
                }

                case 9:
                    CompositeShape comp = new CompositeShape();
                    comp.Children.Add(new GeometricObject(new BoxShape(0.5f * ObjectSize, 1 * ObjectSize, 0.5f * ObjectSize), new Pose(new Vector3F(0, 0.5f * ObjectSize, 0), QuaternionF.Identity)));
                    comp.Children.Add(new GeometricObject(new BoxShape(0.8f * ObjectSize, 0.5f * ObjectSize, 0.5f * ObjectSize), new Pose(new Vector3F(0.3f * ObjectSize, 0.7f * ObjectSize, 0), QuaternionF.CreateRotationZ(-MathHelper.ToRadians(45)))));
                    comp.Children.Add(new GeometricObject(new SphereShape(0.3f * ObjectSize), new Pose(new Vector3F(0, 1.15f * ObjectSize, 0), QuaternionF.Identity)));
                    shape = comp;
                    break;

                case 10:
                    shape = new ConvexHullOfPoints(new[]
                    {
                        new Vector3F(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize),
                        new Vector3F(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize),
                        new Vector3F(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
                        new Vector3F(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
                        new Vector3F(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize)
                    });
                    break;

                case 11:
                    ConvexHullOfShapes shapeHull = new ConvexHullOfShapes();
                    shapeHull.Children.Add(new GeometricObject(new SphereShape(0.3f * ObjectSize), new Pose(new Vector3F(0, 2 * ObjectSize, 0), Matrix33F.Identity)));
                    shapeHull.Children.Add(new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize), Pose.Identity));
                    shape = shapeHull;
                    break;

                case 12:
                    shape = Shape.Empty;
                    break;

                case 13:
                    var numberOfSamplesX = 10;
                    var numberOfSamplesZ = 10;
                    var samples          = new float[numberOfSamplesX * numberOfSamplesZ];
                    for (int z = 0; z < numberOfSamplesZ; z++)
                    {
                        for (int x = 0; x < numberOfSamplesX; x++)
                        {
                            samples[z * numberOfSamplesX + x] = (float)(Math.Cos(z / 3f) * Math.Sin(x / 2f) * BoxSize / 6);
                        }
                    }
                    HeightField heightField = new HeightField(0, 0, 2 * BoxSize, 2 * BoxSize, samples, numberOfSamplesX, numberOfSamplesZ);
                    shape = heightField;
                    break;

                //case 14:
                //shape = new LineShape(new Vector3F(0.1f, 0.2f, 0.3f), new Vector3F(0.1f, 0.2f, -0.3f).Normalized);
                //break;
                case 15:
                    shape = new LineSegmentShape(
                        new Vector3F(0.1f, 0.2f, 0.3f), new Vector3F(0.1f, 0.2f, 0.3f) + 3 * ObjectSize * new Vector3F(0.1f, 0.2f, -0.3f));
                    break;

                case 16:
                    shape = new MinkowskiDifferenceShape
                    {
                        ObjectA = new GeometricObject(new SphereShape(0.1f * ObjectSize)),
                        ObjectB = new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize))
                    };
                    break;

                case 17:
                    shape = new MinkowskiSumShape
                    {
                        ObjectA = new GeometricObject(new SphereShape(0.1f * ObjectSize)),
                        ObjectB = new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize)),
                    };
                    break;

                case 18:
                    shape = new OrthographicViewVolume(0, ObjectSize, 0, ObjectSize, ObjectSize / 2, ObjectSize * 2);
                    break;

                case 19:
                    shape = new PerspectiveViewVolume(MathHelper.ToRadians(60f), 16f / 10, ObjectSize / 2, ObjectSize * 3);
                    break;

                case 20:
                    shape = new PointShape(0.1f, 0.3f, 0.2f);
                    break;

                case 21:
                    shape = new RayShape(new Vector3F(0.2f, 0, -0.12f), new Vector3F(1, 2, 3).Normalized, ObjectSize * 2);
                    break;

                case 22:
                    shape = new RayShape(new Vector3F(0.2f, 0, -0.12f), new Vector3F(1, 2, 3).Normalized, ObjectSize * 2)
                    {
                        StopsAtFirstHit = true
                    };
                    break;

                case 23:
                    shape = new RectangleShape(ObjectSize, ObjectSize * 2);
                    break;

                case 24:
                    shape = new TransformedShape(
                        new GeometricObject(
                            new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize),
                            new Pose(new Vector3F(0.1f, 1, -0.2f))));
                    break;

                case 25:
                    shape = new TriangleShape(
                        new Vector3F(ObjectSize, 0, 0), new Vector3F(0, ObjectSize, 0), new Vector3F(ObjectSize, ObjectSize, ObjectSize));
                    break;
                //case 26:
                //  {
                //    // Create a composite object from which we get the mesh.
                //    CompositeShape compBvh = new CompositeShape();
                //    compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3F(0, 0.5f, 0), Matrix33F.Identity)));
                //    compBvh.Children.Add(
                //      new GeometricObject(
                //        new BoxShape(0.8f, 0.5f, 0.5f),
                //        new Pose(new Vector3F(0.5f, 0.7f, 0), Matrix33F.CreateRotationZ(-(float)MathHelper.ToRadians(15)))));
                //    compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3F(0, 1.15f, 0), Matrix33F.Identity)));
                //    compBvh.Children.Add(
                //      new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3F(0.6f, 1.15f, 0), Matrix33F.CreateRotationX(0.3f))));

                //    TriangleMeshShape meshBvhShape = new TriangleMeshShape { Mesh = compBvh.GetMesh(0.01f, 3) };
                //    meshBvhShape.Partition = new AabbTree<int>();
                //    shape = meshBvhShape;
                //    break;
                //  }
                //case 27:
                //  {
                //    // Create a composite object from which we get the mesh.
                //    CompositeShape compBvh = new CompositeShape();
                //    compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3F(0, 0.5f, 0), QuaternionF.Identity)));
                //    compBvh.Children.Add(
                //      new GeometricObject(
                //        new BoxShape(0.8f, 0.5f, 0.5f),
                //        new Pose(new Vector3F(0.5f, 0.7f, 0), QuaternionF.CreateRotationZ(-(float)MathHelper.ToRadians(15)))));
                //    compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3F(0, 1.15f, 0), QuaternionF.Identity)));
                //    compBvh.Children.Add(
                //      new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3F(0.6f, 1.15f, 0), QuaternionF.CreateRotationX(0.3f))));

                //    TriangleMeshShape meshBvhShape = new TriangleMeshShape { Mesh = compBvh.GetMesh(0.01f, 3) };
                //    meshBvhShape.Partition = new AabbTree<int>();
                //    shape = meshBvhShape;
                //    break;
                //  }
                case 28:
                    shape = new ConvexPolyhedron(new[]
                    {
                        new Vector3F(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize),
                        new Vector3F(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize),
                        new Vector3F(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
                        new Vector3F(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
                        new Vector3F(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize)
                    });
                    break;

                case 29:
                    return;

                default:
                    currentShape++;
                    continue;
                }

                // Create an object with the random shape, pose, color and velocity.
                Pose randomPose = new Pose(
                    random.NextVector3F(-BoxSize + ObjectSize * 2, BoxSize - ObjectSize * 2),
                    random.NextQuaternionF());
                var newObject = new MovingGeometricObject
                {
                    Pose            = randomPose,
                    Shape           = shape,
                    LinearVelocity  = random.NextQuaternionF().Rotate(new Vector3F(MaxLinearVelocity, 0, 0)),
                    AngularVelocity = random.NextQuaternionF().Rotate(Vector3F.Forward)
                                      * RandomHelper.Random.NextFloat(0, MaxAngularVelocity),
                };

                if (RandomHelper.Random.NextBool())
                {
                    newObject.LinearVelocity = Vector3F.Zero;
                }
                if (RandomHelper.Random.NextBool())
                {
                    newObject.AngularVelocity = Vector3F.Zero;
                }

                if (shape is LineShape || shape is HeightField)
                {
                    // Do not move lines or the height field.
                    newObject.LinearVelocity  = Vector3F.Zero;
                    newObject.AngularVelocity = Vector3F.Zero;
                }

                // Create only 1 heightField!
                if (shape is HeightField)
                {
                    if (isFirstHeightField)
                    {
                        isFirstHeightField = true;
                        newObject.Pose     = new Pose(new Vector3F(-BoxSize, -BoxSize, -BoxSize));
                    }
                    else
                    {
                        currentShape++;
                        numberOfObjects = 0;
                        continue;
                    }
                }

                // Add collision object to collision domain.
                _domain.CollisionObjects.Add(new CollisionObject(newObject));

                //co.Type = CollisionObjectType.Trigger;
                //co.Name = "Object" + shape.GetType().Name + "_" + i;
            }
        }
示例#13
0
        public void Test1()
        {
            PlaneRayAlgorithm algo = new PlaneRayAlgorithm(new CollisionDetection());

            // Plane in xz plane.
            CollisionObject a = new CollisionObject(new GeometricObject
            {
                Shape = new PlaneShape(Vector3F.UnitY, 1),
                Pose  = new Pose(new Vector3F(0, -1, 0)),
            });

            // Ray
            CollisionObject b = new CollisionObject(new GeometricObject
            {
                Shape = new RayShape(new Vector3F(0, 0, 0), new Vector3F(-1, 0, 0), 100),
                Pose  = new Pose(new Vector3F(0, 1, 0)),
            });

            // Separated
            Assert.AreEqual(false, algo.HaveContact(a, b));
            Assert.AreEqual(false, algo.HaveContact(b, a));
            Assert.AreEqual(0, algo.GetContacts(a, b).Count);
            Assert.AreEqual(-1, algo.GetClosestPoints(a, b)[0].PenetrationDepth);

            // Contained
            ((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(0, -1, 0));
            Assert.AreEqual(true, algo.HaveContact(a, b));
            Assert.AreEqual(true, algo.HaveContact(b, a));
            Assert.AreEqual(1, algo.GetContacts(a, b).Count);
            Assert.AreEqual(0, algo.GetClosestPoints(a, b)[0].PenetrationDepth);

            // Touching
            ((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0));
            Assert.AreEqual(true, algo.HaveContact(a, b));
            Assert.AreEqual(true, algo.HaveContact(b, a));
            Assert.AreEqual(1, algo.GetContacts(a, b).Count);
            Assert.AreEqual(0, algo.GetClosestPoints(a, b)[0].PenetrationDepth);


            // Shooting into plane.
            ((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(0, 1, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
            Assert.AreEqual(1, algo.GetContacts(a, b).Count);
            Assert.AreEqual(1, algo.GetContacts(b, a)[0].PenetrationDepth);
            Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(-1, 0, 0), algo.GetContacts(b, a)[0].PositionALocal));
            Assert.AreEqual(1, algo.GetClosestPoints(a, b)[0].PenetrationDepth);
        }
示例#14
0
        public void TestTouchingContact()
        {
            PlaneSphereAlgorithm algo = new PlaneSphereAlgorithm(new CollisionDetection());

            CollisionObject objectA = new CollisionObject();

            ((GeometricObject)objectA.GeometricObject).Shape = new SphereShape(1);
            ((GeometricObject)objectA.GeometricObject).Pose  = new Pose(new Vector3F(10, -3, 20));

            CollisionObject objectB = new CollisionObject();

            ((GeometricObject)objectB.GeometricObject).Shape = new PlaneShape(new Vector3F(0, 1, 0).Normalized, 2);
            ((GeometricObject)objectB.GeometricObject).Pose  = new Pose(new Vector3F(1, 0, 0), QuaternionF.CreateRotationZ(-(float)Math.PI));

            ContactSet cs = ContactSet.Create(objectA, objectB);

            algo.UpdateContacts(cs, 0);

            Assert.AreEqual(objectA, cs.ObjectA);
            Assert.AreEqual(objectB, cs.ObjectB);
            Assert.AreEqual(1, cs.Count);
            Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(10, -2, 20), cs[0].Position));
            Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 1, 0), cs[0].Normal));
            Assert.IsTrue(Numeric.AreEqual(0, cs[0].PenetrationDepth));
        }
示例#15
0
        public void ComputeCollision()
        {
            CollisionObject a     = new CollisionObject();
            CompositeShape  compo = new CompositeShape();

            compo.Children.Add(new GeometricObject(new SphereShape(2), Pose.Identity));
            compo.Children.Add(new GeometricObject(new SphereShape(1), new Pose(new Vector3F(0, 3, 0))));
            a.GeometricObject = new GeometricObject(compo, Pose.Identity);

            CollisionObject b = new CollisionObject(new GeometricObject
            {
                Shape = new PlaneShape(new Vector3F(0, 1, 0), 1),
                Pose  = new Pose(new Vector3F(0, -1, 0)),
            });

            ContactSet set;
            CompositeShapeAlgorithm algo = new CompositeShapeAlgorithm(new CollisionDetection());

            set = algo.GetClosestPoints(a, b);
            Assert.AreEqual(true, algo.HaveContact(a, b));
            Assert.AreEqual(true, algo.HaveContact(b, a));
            Assert.AreEqual(1, set.Count);
            Assert.AreEqual(2, set[0].PenetrationDepth);
            Assert.AreEqual(new Vector3F(0, -2, 0), set[0].PositionAWorld);

            ((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 2, 3), a.GeometricObject.Pose.Orientation);
            set = set.Swapped;
            algo.UpdateContacts(set, 0);
            Assert.AreEqual(true, algo.HaveContact(a, b));
            Assert.AreEqual(true, algo.HaveContact(b, a));
            Assert.AreEqual(1, set.Count);
            Assert.AreEqual(0, set[0].PenetrationDepth);
            Assert.AreEqual(new Vector3F(0, 0, 3), set[0].PositionBWorld);

            ((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
            set = set.Swapped; // new order: (a, b)
            algo.UpdateContacts(set, 0);
            Assert.AreEqual(true, algo.HaveContact(a, b));
            Assert.AreEqual(2, set.Count);
            Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(-2, 0, 0), set[0].PositionALocal));
            Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(-1, 3, 0), set[1].PositionALocal));
            Assert.AreEqual(new Vector3F(0, -1, 0), set[0].Normal);
            Assert.AreEqual(new Vector3F(0, -1, 0), set[1].Normal);
            Assert.AreEqual(2, set[0].PenetrationDepth);
            Assert.IsTrue(Numeric.AreEqual(1, set[1].PenetrationDepth));

            algo.UpdateClosestPoints(set, 0);
            Assert.AreEqual(1, set.Count);

            ((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 2.1f, 0), a.GeometricObject.Pose.Orientation);
            algo.UpdateContacts(set, 0);
            Assert.AreEqual(false, algo.HaveContact(a, b));
            Assert.AreEqual(0, set.Count);
            algo.UpdateClosestPoints(set, 0);
            Assert.AreEqual(1, set.Count);
            Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(-2, 0, 0), set[0].PositionALocal));
            Assert.AreEqual(new Vector3F(0, -1, 0), set[0].Normal);
        }
        public void ComputeCollisionLineLine()
        {
            CollisionObject ray = new CollisionObject(new GeometricObject
            {
                Shape = new RayShape(new Vector3F(0, 0, -1), new Vector3F(1, 0, 0), 10),
                Pose  = new Pose(new Vector3F(0, 0, 1)),
            });

            CollisionObject sphere = new CollisionObject(new GeometricObject
            {
                Shape = new SphereShape(1),
                Pose  = new Pose(RandomHelper.Random.NextQuaternionF()),
            });

            RaySphereAlgorithm algo = new RaySphereAlgorithm(new CollisionDetection());

            ContactSet set;

            set = algo.GetClosestPoints(ray, sphere);
            Assert.AreEqual(0, set[0].PenetrationDepth);
            Assert.AreEqual(Vector3F.UnitY, set[0].Normal);
            Assert.AreEqual(true, algo.HaveContact(ray, sphere));

            set = set.Swapped;
            algo.UpdateContacts(set, 0);
            Assert.AreEqual(0, set[0].PenetrationDepth);
            Assert.AreEqual(-Vector3F.UnitY, set[0].Normal);

            ((GeometricObject)sphere.GeometricObject).Pose = new Pose(new Vector3F(10, 0, 0), sphere.GeometricObject.Pose.Orientation);
            set = algo.GetClosestPoints(sphere, ray);
            Assert.IsTrue(Numeric.AreEqual(9, set[0].PenetrationDepth));
            Assert.IsTrue(Vector3F.AreNumericallyEqual(-Vector3F.UnitX, set[0].Normal));
            Assert.AreEqual(true, algo.HaveContact(ray, sphere));

            ((GeometricObject)ray.GeometricObject).Pose = new Pose(new Vector3F(0, -2, 1), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
            set = algo.GetClosestPoints(sphere, ray);
            Assert.IsTrue(Numeric.AreEqual(-9, set[0].PenetrationDepth));
            Assert.IsTrue(Vector3F.AreNumericallyEqual(-Vector3F.UnitX, set[0].Normal));
            Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(4.5f, 0, 0), set[0].Position));
            Assert.AreEqual(false, algo.HaveContact(ray, sphere));

            algo.UpdateContacts(set, 0);
            Assert.AreEqual(0, set.Count);

            // Degenerate case: sphere radius is 0.
            ((GeometricObject)sphere.GeometricObject).Shape = new SphereShape(0);
            ((GeometricObject)ray.GeometricObject).Pose     = new Pose(new Vector3F(0, 0, 1));
            algo.UpdateClosestPoints(set, 0);
            Assert.AreEqual(0, set[0].PenetrationDepth);
            Assert.AreEqual(-Vector3F.UnitY, set[0].Normal);
            Assert.AreEqual(true, algo.HaveContact(ray, sphere));
        }
        public void RayCastStopsAtFirstHit()
        {
            CollisionDomain domain = new CollisionDomain(new CollisionDetection());

            CollisionObject ray = new CollisionObject();

            ((GeometricObject)ray.GeometricObject).Shape = new RayShape(new Vector3F(), new Vector3F(1, 0, 0), 100)
            {
                StopsAtFirstHit = true,
            };
            //ray.Name = "Ray";

            CollisionObject b = new CollisionObject();

            ((GeometricObject)b.GeometricObject).Shape = new SphereShape(1);
            ((GeometricObject)b.GeometricObject).Pose  = new Pose(new Vector3F(-10, 0, 0f));
            //b.Name = "b";

            CollisionObject c = new CollisionObject();

            ((GeometricObject)c.GeometricObject).Shape = new SphereShape(1);
            ((GeometricObject)c.GeometricObject).Pose  = new Pose(new Vector3F(0, 0, 0f));
            //c.Name = "c";

            CollisionObject d = new CollisionObject();

            ((GeometricObject)d.GeometricObject).Shape = new SphereShape(1);
            ((GeometricObject)d.GeometricObject).Pose  = new Pose(new Vector3F(10, 0, 0f));
            //d.Name = "d";

            CollisionObject e = new CollisionObject();

            ((GeometricObject)e.GeometricObject).Shape = new SphereShape(1);
            ((GeometricObject)e.GeometricObject).Pose  = new Pose(new Vector3F(20, 0, 0f));
            //e.Name = "e";

            CollisionObject f = new CollisionObject();

            ((GeometricObject)f.GeometricObject).Shape = new SphereShape(1);
            ((GeometricObject)f.GeometricObject).Pose  = new Pose(new Vector3F(110, 0, 0f));
            //f.Name = "f";

            // Positions: b=-10, c=0, d=10, e=20, f=110
            domain.CollisionObjects.Add(ray);
            domain.CollisionObjects.Add(b);
            domain.CollisionObjects.Add(d);
            domain.CollisionObjects.Add(c);
            domain.CollisionObjects.Add(e);
            domain.CollisionObjects.Add(f);

            domain.Update(0.01f);
            Assert.AreEqual(1, domain.GetContacts(ray).Count());
            Assert.AreEqual(true, domain.HaveContact(ray, c));

            ((GeometricObject)c.GeometricObject).Pose = new Pose(new Vector3F(30));
            // Positions: b=-10, d=10, e=20, c=30, f=110
            domain.Update(0.01f);
            Assert.AreEqual(1, domain.GetContacts(ray).Count());
            Assert.AreEqual(true, domain.HaveContact(ray, d));

            ((GeometricObject)d.GeometricObject).Pose = new Pose(new Vector3F(40));
            // Positions: b=-10, e=20, c=30, d=40, f=110
            domain.Update(0.01f);
            Assert.AreEqual(1, domain.GetContacts(ray).Count());
            Assert.AreEqual(true, domain.HaveContact(ray, e));

            ((GeometricObject)ray.GeometricObject).Pose = new Pose(((GeometricObject)ray.GeometricObject).Pose.Position, QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
            domain.Update(0.01f);
            Assert.AreEqual(0, domain.GetContacts(ray).Count());

            ((GeometricObject)ray.GeometricObject).Pose = new Pose(((GeometricObject)ray.GeometricObject).Pose.Position, QuaternionF.CreateRotationZ(ConstantsF.Pi));
            domain.Update(0.01f);
            Assert.AreEqual(1, domain.GetContacts(ray).Count());
            Assert.AreEqual(true, domain.HaveContact(ray, b));

            ((GeometricObject)ray.GeometricObject).Pose = new Pose(((GeometricObject)ray.GeometricObject).Pose.Position, QuaternionF.Identity);
            domain.Update(0.01f);

            // Positions: b=-10, e=20, c=30, d=40, f=110
            CollisionObject gNotInDomain = new CollisionObject();

            ((GeometricObject)gNotInDomain.GeometricObject).Shape = new SphereShape(1);
            ((GeometricObject)gNotInDomain.GeometricObject).Pose  = new Pose(new Vector3F(10, 0, 0f));
            Assert.AreEqual(true, domain.HaveContact(ray, gNotInDomain));
            Assert.AreEqual(1, domain.GetContacts(gNotInDomain).Count());
            Assert.AreEqual(1, domain.GetContacts(ray, gNotInDomain).Count);
            Assert.AreEqual(true, domain.HasContact(gNotInDomain));
            ((GeometricObject)gNotInDomain.GeometricObject).Pose = new Pose(new Vector3F(25, 0, 0f)); // behind e
            Assert.AreEqual(false, domain.HaveContact(ray, gNotInDomain));
            Assert.AreEqual(false, domain.HaveContact(gNotInDomain, ray));
            Assert.AreEqual(false, domain.HasContact(gNotInDomain));
            Assert.AreEqual(0, domain.GetContacts(gNotInDomain).Count());
            Assert.IsNull(domain.GetContacts(ray, gNotInDomain));
            Assert.IsNull(domain.GetContacts(gNotInDomain, ray));

            // Remove ray from domain.
            domain.CollisionObjects.Remove(ray);
            domain.Update(0.01f);
            Assert.AreEqual(0, domain.ContactSets.Count);

            // Positions: b=-10, e=20, g=25, c=30, d=40, f=110
            domain.Update(0.01f);
            Assert.AreEqual(1, domain.GetContacts(ray).Count());
            Assert.AreEqual(true, domain.HaveContact(ray, e));
            Assert.AreEqual(false, domain.HaveContact(ray, c));
            Assert.AreEqual(false, domain.HaveContact(ray, gNotInDomain));
            Assert.IsNull(domain.GetContacts(ray, gNotInDomain));
        }
示例#18
0
        public void ComputeCollisionLineLine()
        {
            CollisionObject line0 = new CollisionObject(new GeometricObject
            {
                Shape = new LineShape(new Vector3F(1, 2, 3), new Vector3F(1, 0, 0)),
            });
            //line0.Name = "line0";

            CollisionObject line1 = new CollisionObject(new GeometricObject
            {
                Shape = new LineShape(new Vector3F(0, 0, 1), new Vector3F(1, 0, 0)),
            });
            //line1.Name = "line1";

            LineAlgorithm algo = new LineAlgorithm(new CollisionDetection());

            ContactSet set;

            ((GeometricObject)line1.GeometricObject).Pose = new Pose(new Vector3F(1, 2, 2), line1.GeometricObject.Pose.Orientation);
            set = algo.GetClosestPoints(line0, line1);
            Assert.AreEqual(0, set[0].PenetrationDepth);
            Assert.AreEqual(Vector3F.UnitY, set[0].Normal);
            Assert.AreEqual(true, algo.HaveContact(line0, line1));

            ((GeometricObject)line1.GeometricObject).Pose = new Pose(line1.GeometricObject.Pose.Position, QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
            set = algo.GetClosestPoints(line0, line1);
            Assert.IsTrue(Numeric.AreEqual(0, set[0].PenetrationDepth));
            Assert.AreEqual(new Vector3F(1, 2, 3), set[0].Position);
            Assert.IsTrue(Vector3F.AreNumericallyEqual(Vector3F.UnitZ, set[0].Normal));
            Assert.AreEqual(true, algo.HaveContact(line0, line1));

            ((GeometricObject)line1.GeometricObject).Pose = new Pose(new Vector3F(1, 2, 4), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
            set = algo.GetClosestPoints(line1, line0);
            Assert.IsTrue(Numeric.AreEqual(-2, set[0].PenetrationDepth));
            Assert.AreEqual(new Vector3F(1, 2, 4), set[0].Position);
            Assert.AreEqual(-Vector3F.UnitZ, set[0].Normal);
            Assert.AreEqual(false, algo.HaveContact(line0, line1));

            algo.UpdateContacts(set, 0.01f);
            Assert.AreEqual(0, set.Count);
        }