Beispiel #1
0
 public void GetAxisAlignedBoundingBox()
 {
     Assert.AreEqual(new Aabb(), new BoxShape().GetAabb(Pose.Identity));
     Assert.AreEqual(new Aabb(new Vector3F(10, 100, -13), new Vector3F(10, 100, -13)),
                     new BoxShape().GetAabb(new Pose(new Vector3F(10, 100, -13),
                                                     QuaternionF.CreateRotation(new Vector3F(1, 1, 1), 0.7f))));
     Assert.AreEqual(new Aabb(new Vector3F(5, 90, 985), new Vector3F(15, 110, 1015)),
                     new BoxShape(10, 20, 30).GetAabb(new Pose(new Vector3F(10, 100, 1000),
                                                               QuaternionF.Identity)));
     // TODO: Test rotations.
 }
Beispiel #2
0
 public void GetAxisAlignedBoundingBox()
 {
     Assert.AreEqual(new Aabb(), new CapsuleShape().GetAabb(Pose.Identity));
     Assert.AreEqual(new Aabb(new Vector3F(10, 100, -13), new Vector3F(10, 100, -13)),
                     new CapsuleShape().GetAabb(new Pose(new Vector3F(10, 100, -13),
                                                         QuaternionF.CreateRotation(new Vector3F(1, 1, 1), 0.7f))));
     Assert.AreEqual(new Aabb(new Vector3F(0, 80, 990), new Vector3F(20, 120, 1010)),
                     new CapsuleShape(10, 40).GetAabb(new Pose(new Vector3F(10, 100, 1000),
                                                               QuaternionF.Identity)));
     // TODO: Test rotations.
 }
        // Add light sources for standard three-point lighting.
        private static void AddLights(Scene scene)
        {
            var ambientLight = new AmbientLight
            {
                Color     = new Vector3F(0.05333332f, 0.09882354f, 0.1819608f),
                Intensity = 1,
                HemisphericAttenuation = 0,
            };

            scene.Children.Add(new LightNode(ambientLight));

            var keyLight = new DirectionalLight
            {
                Color             = new Vector3F(1, 0.9607844f, 0.8078432f),
                DiffuseIntensity  = 1,
                SpecularIntensity = 1,
            };
            var keyLightNode = new LightNode(keyLight)
            {
                Name      = "KeyLight",
                Priority  = 10, // This is the most important light.
                PoseWorld = new Pose(QuaternionF.CreateRotation(Vector3F.Forward, new Vector3F(-0.5265408f, -0.5735765f, -0.6275069f))),
            };

            scene.Children.Add(keyLightNode);

            var fillLight = new DirectionalLight
            {
                Color             = new Vector3F(0.9647059f, 0.7607844f, 0.4078432f),
                DiffuseIntensity  = 1,
                SpecularIntensity = 0,
            };
            var fillLightNode = new LightNode(fillLight)
            {
                Name      = "FillLight",
                PoseWorld = new Pose(QuaternionF.CreateRotation(Vector3F.Forward, new Vector3F(0.7198464f, 0.3420201f, 0.6040227f))),
            };

            scene.Children.Add(fillLightNode);

            var backLight = new DirectionalLight
            {
                Color             = new Vector3F(0.3231373f, 0.3607844f, 0.3937255f),
                DiffuseIntensity  = 1,
                SpecularIntensity = 1,
            };
            var backLightNode = new LightNode(backLight)
            {
                Name      = "BackLight",
                PoseWorld = new Pose(QuaternionF.CreateRotation(Vector3F.Forward, new Vector3F(0.4545195f, -0.7660444f, 0.4545195f))),
            };

            scene.Children.Add(backLightNode);
        }
Beispiel #4
0
        private static void Append(StringBuilder text, CollisionObject co, bool isFirstCollisionObject)
        {
            text.Append(@"
      {
        var mesh = new TriangleMesh();");

            var shape = co.GeometricObject.Shape as TriangleMeshShape;

            if (shape == null)
            {
                throw new NotSupportedException("The shapes must be TriangleMeshShapes");
            }

            var mesh = shape.Mesh;

            for (int i = 0; i < mesh.NumberOfTriangles; i++)
            {
                text.Append(@"
        mesh.Add(");
                Append(text, mesh.GetTriangle(i));
                text.Append(");");
            }

            text.Append(@"

        var pose = new Pose(");
            Append(text, co.GeometricObject.Pose.Position);
            text.Append(", ");
            Append(text, QuaternionF.CreateRotation(co.GeometricObject.Pose.Orientation));
            text.Append(@");
        var scale = ");
            Append(text, co.GeometricObject.Scale);
            text.Append(@";
        var shape = new TriangleMeshShape(mesh) { Partition = new CompressedAabbTree() };
        shape.IsTwoSided = ");
            Append(text, shape.IsTwoSided);
            text.Append(@";
        shape.EnableContactWelding = ");
            Append(text, shape.EnableContactWelding);
            text.Append(@";
        ");

            if (isFirstCollisionObject)
            {
                text.Append("_objectA");
            }
            else
            {
                text.Append("_objectB");
            }

            text.Append(@" = new CollisionObject(new GeometricObject(shape, scale, pose));
      }");
        }
        public void Angle()
        {
            Vector3F    axis = new Vector3F(1.0f, 2.0f, 3.0f);
            QuaternionF q    = QuaternionF.CreateRotation(axis, 0.4f);

            Assert.IsTrue(Numeric.AreEqual(0.4f, q.Angle));
            q.Angle = 0.9f;
            Assert.IsTrue(QuaternionF.AreNumericallyEqual(q, QuaternionF.CreateRotation(axis, 0.9f)));

            Assert.AreEqual(0, new QuaternionF(1.000001f, 0, 0, 0).Angle);
        }
        public void SlerpNegatedSinglePrecision()
        {
            QuaternionF q1    = new QuaternionF(-1.0f, 0.0f, 0.0f, 0.0f);
            QuaternionF q2    = QuaternionF.CreateRotation(-Vector3F.UnitZ, (float)-Math.PI / 2);
            QuaternionF slerp = InterpolationHelper.Slerp(q1, q2, 0.5f);

            Assert.IsTrue(slerp.IsNumericallyNormalized);
            Vector3F v      = slerp.Rotate(Vector3F.UnitX);
            Vector3F result = new Vector3F(1.0f, 1.0f, 0.0f).Normalized;

            Assert.IsTrue(Vector3F.AreNumericallyEqual(result, v));
        }
        public void RotationMatrix33()
        {
            float       angle   = -1.6f;
            Vector3F    axis    = new Vector3F(1.0f, 2.0f, -3.0f);
            QuaternionF q       = QuaternionF.CreateRotation(axis, angle);
            Matrix33F   m33     = Matrix33F.CreateRotation(axis, angle);
            Vector3F    v       = new Vector3F(0.3f, -2.4f, 5.6f);
            Vector3F    result1 = q.ToRotationMatrix33() * v;
            Vector3F    result2 = m33 * v;

            Assert.IsTrue(Vector3F.AreNumericallyEqual(result1, result2));
        }
Beispiel #8
0
 private void OnMouseMove(object sender, MouseEventArgs eventArgs)
 {
     if (eventArgs.LeftButton == MouseButtonState.Pressed)
     {
         // Rotate view.
         Point     mousePosition = eventArgs.GetPosition(AssociatedObject);
         Vector3F  dragVector    = MapToSphere(mousePosition);
         Matrix44F rotation      = QuaternionF.CreateRotation(_startVector, dragVector).ToRotationMatrix44();
         _transform = rotation * _originalTransform;
         UpdateCamera(_cameraNode);
     }
 }
Beispiel #9
0
        public void CreateRotationZ()
        {
            float     angle = (float)MathHelper.ToRadians(30);
            Matrix33F m     = Matrix33F.CreateRotationZ(angle);

            Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F((float)Math.Cos(angle), (float)Math.Sin(angle), 0), m * Vector3F.UnitX));

            QuaternionF q = QuaternionF.CreateRotation(Vector3F.UnitZ, angle);

            Assert.IsTrue(Vector3F.AreNumericallyEqual(q.Rotate(Vector3F.One), m * Vector3F.One));

            Assert.IsTrue(Matrix33F.AreNumericallyEqual(Matrix33F.CreateRotation(Vector3F.UnitZ, angle), m));
        }
Beispiel #10
0
        /// <summary>
        /// Interpolates two poses.
        /// </summary>
        /// <param name="startPose">The start pose.</param>
        /// <param name="endPose">The end pose.</param>
        /// <param name="parameter">
        /// The interpolation parameter. If the value is 0, the <paramref name="startPose"/> is
        /// returned. If the value is 1, the <paramref name="endPose"/> is returned. For values between
        /// 0 and 1 an interpolated pose is returned.
        /// </param>
        /// <returns>An interpolated pose.</returns>
        public static Pose Interpolate(Pose startPose, Pose endPose, float parameter)
        {
            // Linearly interpolate position.
            var interpolatedPosition = startPose.Position * (1 - parameter) + endPose.Position * parameter;

            // Slerp orientation.
            var interpolatedOrientation = InterpolationHelper.Lerp(
                QuaternionF.CreateRotation(startPose.Orientation),
                QuaternionF.CreateRotation(endPose.Orientation),
                parameter);

            return(new Pose(interpolatedPosition, interpolatedOrientation));
        }
Beispiel #11
0
        /// <summary>
        /// Moves and rotates the scene node so that it faces a certain direction (in world space).
        /// </summary>
        /// <param name="node">The scene node.</param>
        /// <param name="position">The new position in world space.</param>
        /// <param name="target">
        /// The target coordinates in world space at which the scene node is "looking".
        /// </param>
        /// <param name="upVector">
        /// The direction that is "up" from the scene node's point of view given in world space. (Does
        /// not need to be normalized.)
        /// </param>
        /// <remarks>
        /// A scene node uses the same coordinate system as the <strong>XNA Framework</strong>:
        /// By default, the positive x-axis points to the right, the positive y-axis points up, and the
        /// positive z-axis points towards the viewer. This method moves the scene node to
        /// <paramref name="position"/> and rotates it so that its local forward direction (0, 0, -1) is
        /// pointing towards <paramref name="target"/>.
        /// </remarks>
        /// <exception cref="ArgumentNullException">
        /// <paramref name="node"/> is <see langword="null"/>.
        /// </exception>
        /// <exception cref="ArgumentException">
        /// <paramref name="position"/> is the same as <paramref name="target"/>.
        /// </exception>
        /// <exception cref="ArgumentException">
        /// <paramref name="upVector"/> is (0, 0, 0).
        /// </exception>
        /// <exception cref="DivideByZeroException">
        /// The camera direction (<paramref name="target"/> - <paramref name="position"/>) is probably
        /// pointing in the same or opposite direction as <paramref name="upVector"/>. (The two vectors
        /// must not be parallel.)
        /// </exception>
        public static void LookAt(this SceneNode node, Vector3F position, Vector3F target, Vector3F upVector)
        {
            if (node == null)
            {
                throw new ArgumentNullException("node");
            }

            Matrix44F   view        = Matrix44F.CreateLookAt(position, target, upVector);
            Matrix44F   viewInverse = view.Inverse;
            QuaternionF orientation = QuaternionF.CreateRotation(viewInverse.Minor);

            node.PoseWorld = new Pose(position, orientation);
        }
Beispiel #12
0
        public void Update(float deltaTime, Matrix44F world)
        {
            if (deltaTime <= 0)
            {
                return;
            }

            // Reset bone transform.
            SkeletonPose.SetBoneTransform(BoneIndex, SrtTransform.Identity);

            // Get new fixed point position in world space.
            var bonePoseAbsolute   = SkeletonPose.GetBonePoseAbsolute(BoneIndex);
            var bonePoseWorld      = world * bonePoseAbsolute;
            var fixedPointPosition = bonePoseWorld.TransformPosition(Offset);

            // If we haven't set the fixed point position before, then store the position
            // and we are done.
            if (_fixedPointPosition.IsNaN)
            {
                _fixedPointPosition = fixedPointPosition;
                return;
            }

            // New position and velocity of fixed point.
            _fixedPointVelocity = (fixedPointPosition - _fixedPointPosition) / deltaTime;
            _fixedPointPosition = fixedPointPosition;

            // If the particle position was not set before, then we only store the current values.
            // The real work starts in the next frame.
            if (_particlePosition.IsNaN)
            {
                _particlePosition = _fixedPointPosition;
                _particleVelocity = _fixedPointVelocity;
                return;
            }

            // Compute the spring force between the particle and the fixed point.
            var force = Spring * (_fixedPointPosition - _particlePosition) + Damping * (_fixedPointVelocity - _particleVelocity);

            // Update velocity and position of the particle using symplectic Euler.
            _particleVelocity = _particleVelocity + force * deltaTime;
            _particlePosition = _particlePosition + _particleVelocity * deltaTime;

            // Convert particle position back to bone space.
            var particleLocal = bonePoseWorld.Inverse.TransformPosition(_particlePosition);

            // Create rotation between the fixed point vector and the particle vector.
            var boneTransform = new SrtTransform(QuaternionF.CreateRotation(Offset, particleLocal));

            SkeletonPose.SetBoneTransform(BoneIndex, boneTransform);
        }
 public void GetAxisAlignedBoundingBox()
 {
     Assert.AreEqual(new Aabb(), new RectangleShape().GetAabb(Pose.Identity));
     Assert.AreEqual(new Aabb(new Vector3F(10, 100, -13), new Vector3F(10, 100, -13)),
                     new RectangleShape().GetAabb(new Pose(new Vector3F(10, 100, -13),
                                                           QuaternionF.CreateRotation(new Vector3F(1, 1, 1), 0.7f))));
     Assert.AreEqual(new Aabb(new Vector3F(5, 90, 1000), new Vector3F(15, 110, 1000)),
                     new RectangleShape(10, 20).GetAabb(new Pose(new Vector3F(10, 100, 1000),
                                                                 QuaternionF.Identity)));
     Assert.AreEqual(new Aabb(new Vector3F(5, 100, 990), new Vector3F(15, 100, 1010)),
                     new RectangleShape(10, 20).GetAabb(new Pose(new Vector3F(10, 100, 1000),
                                                                 QuaternionF.CreateRotationX(ConstantsF.PiOver2))));
     // TODO: Test complex rotations.
 }
Beispiel #14
0
        private static void DoWork(QuaternionF skeletonOffset, SkeletonPose skeletonA, SkeletonPose skeletonB, int boneIndexA, int neckBoneIndexA, int leftShoulderBoneIndexA, int rightShoulderBoneIndexA, int boneIndexB, int neckBoneIndexB, int leftShoulderBoneIndexB, int rightShoulderBoneIndexB)
        {
            // Reset root bone.
            skeletonB.ResetBoneTransforms(boneIndexB, boneIndexB, false, true, false);

            // Get absolute positions all bones.
            var boneA          = skeletonA.GetBonePoseAbsolute(boneIndexA).Translation;
            var neckA          = skeletonA.GetBonePoseAbsolute(neckBoneIndexA).Translation;
            var leftShoulderA  = skeletonA.GetBonePoseAbsolute(leftShoulderBoneIndexA).Translation;
            var rightShoulderA = skeletonA.GetBonePoseAbsolute(rightShoulderBoneIndexA).Translation;
            var boneB          = skeletonB.GetBonePoseAbsolute(boneIndexB).Translation;
            var neckB          = skeletonB.GetBonePoseAbsolute(neckBoneIndexB).Translation;
            var leftShoulderB  = skeletonB.GetBonePoseAbsolute(leftShoulderBoneIndexB).Translation;
            var rightShoulderB = skeletonB.GetBonePoseAbsolute(rightShoulderBoneIndexB).Translation;

            // Abort if any bone to bone distance is 0.
            if (Vector3F.AreNumericallyEqual(boneA, neckA) ||
                Vector3F.AreNumericallyEqual(boneA, rightShoulderA) ||
                Vector3F.AreNumericallyEqual(leftShoulderA, rightShoulderA) ||
                Vector3F.AreNumericallyEqual(boneB, neckB) ||
                Vector3F.AreNumericallyEqual(boneB, rightShoulderB) ||
                Vector3F.AreNumericallyEqual(leftShoulderB, rightShoulderB))
            {
                return;
            }

            // Get shoulder axis vectors in model B space.
            var shoulderAxisA = rightShoulderA - leftShoulderA;

            shoulderAxisA = skeletonOffset.Rotate(shoulderAxisA);
            var shoulderAxisB = rightShoulderB - leftShoulderB;

            // Create a twist rotation from the shoulder vectors.
            var shoulderRotation = QuaternionF.CreateRotation(shoulderAxisB, shoulderAxisA);

            // Apply this twist to the spine. (Modifies the neckB position.)
            neckB = boneB + shoulderRotation.Rotate(neckB - boneB);

            // Get spine vectors in model B space.
            var spineAxisA = neckA - boneA;

            spineAxisA = skeletonOffset.Rotate(spineAxisA);
            var spineAxisB = neckB - boneB;

            // Create swing rotation from spine vectors.
            var spineRotation = QuaternionF.CreateRotation(spineAxisB, spineAxisA);

            // Apply the shoulder twist rotation followed by the spine swing rotation.
            skeletonB.RotateBoneAbsolute(boneIndexB, spineRotation * shoulderRotation);
        }
Beispiel #15
0
        public void GetAxisAlignedBoundingBox()
        {
            float nInf = float.NegativeInfinity;
            float pInf = float.PositiveInfinity;

            Assert.AreEqual(new Aabb(new Vector3F(nInf, 0, 0), new Vector3F(pInf, 0, 0)), new LineShape().GetAabb(Pose.Identity));
            Assert.AreEqual(new Aabb(new Vector3F(nInf), new Vector3F(pInf)),
                            new LineShape().GetAabb(new Pose(new Vector3F(10, 100, -13),
                                                             QuaternionF.CreateRotation(new Vector3F(1, 1, 1), 0.7f))));
            Assert.AreEqual(new Aabb(new Vector3F(11, nInf, 1003), new Vector3F(11, pInf, 1003)),
                            new LineShape(new Vector3F(1, 2, 3), new Vector3F(0, -1, 0)).GetAabb(new Pose(new Vector3F(10, 100, 1000),
                                                                                                          QuaternionF.Identity)));
            // TODO: Test rotations.
        }
        //--------------------------------------------------------------
        #region Methods
        //--------------------------------------------------------------

        private void InitializeBody(Vector3F upVector)
        {
            if (!upVector.TryNormalize())
            {
                throw new ArgumentException("The up vector must not be a zero vector.");
            }

            UpVector = upVector;

            CapsuleShape shape = new CapsuleShape(0.4f, 1.8f);
            MassFrame    mass  = new MassFrame {
                Mass = 100
            };

            UniformMaterial material = new UniformMaterial
            {
                // The body should be frictionless, so that it can be easily pushed by the simulation to
                // valid positions.
                StaticFriction  = 0.0f,
                DynamicFriction = 0.0f,

                // The body should not bounce when being hit or pushed.
                Restitution = 0
            };

            Body = new RigidBody(shape, mass, material)
            {
                // We set the mass explicitly and it should not automatically change when the
                // shape is changed; e.g. a ducked character has a smaller shape, but still the same mass.
                AutoUpdateMass = false,

                // This body is under our control and should never be deactivated by the simulation.
                CanSleep   = false,
                CcdEnabled = true,

                // The capsule does not rotate in any direction.
                LockRotationX = true,
                LockRotationY = true,
                LockRotationZ = true,

                Name = "CharacterController",

                Pose = new Pose(shape.Height / 2 * upVector,
                                QuaternionF.CreateRotation(Vector3F.UnitY, upVector)),
            };

            // When the user changes the shape, we must re-compute all contacts.
            Body.ShapeChanged += (s, e) => UpdateContacts();
        }
Beispiel #17
0
        public void GetAxisAlignedBoundingBox()
        {
            Assert.AreEqual(new Aabb(), new PointShape().GetAabb(Pose.Identity));
            Assert.AreEqual(new Aabb(new Vector3F(10, 100, -13), new Vector3F(10, 100, -13)),
                            new PointShape().GetAabb(new Pose(new Vector3F(10, 100, -13),
                                                              QuaternionF.CreateRotation(new Vector3F(1, 1, 1), 0.7f))));
            Assert.AreEqual(new Aabb(new Vector3F(11, 102, 1003), new Vector3F(11, 102, 1003)),
                            new PointShape(new Vector3F(1, 2, 3)).GetAabb(new Pose(new Vector3F(10, 100, 1000),
                                                                                   QuaternionF.Identity)));
            QuaternionF rotation = QuaternionF.CreateRotation(new Vector3F(1, 1, 1), 0.7f);
            Vector3F    worldPos = rotation.Rotate(new Vector3F(1, 2, 3)) + new Vector3F(10, 100, 1000);

            Assert.IsTrue(Vector3F.AreNumericallyEqual(worldPos, new PointShape(new Vector3F(1, 2, 3)).GetAabb(new Pose(new Vector3F(10, 100, 1000), rotation)).Minimum));
            Assert.IsTrue(Vector3F.AreNumericallyEqual(worldPos, new PointShape(new Vector3F(1, 2, 3)).GetAabb(new Pose(new Vector3F(10, 100, 1000), rotation)).Maximum));
        }
        public void QuaternionFromMatrix33()
        {
            Vector3F    v = Vector3F.One;
            Matrix33F   m = Matrix33F.Identity;
            QuaternionF q = QuaternionF.CreateRotation(m);

            Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));

            m = Matrix33F.CreateRotation(Vector3F.UnitX, 0.3f);
            q = QuaternionF.CreateRotation(m);
            Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));

            m = Matrix33F.CreateRotation(Vector3F.UnitY, 1.0f);
            q = QuaternionF.CreateRotation(m);
            Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));

            m = Matrix33F.CreateRotation(Vector3F.UnitZ, 4.0f);
            q = QuaternionF.CreateRotation(m);
            Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));

            m = Matrix33F.Identity;
            q = QuaternionF.CreateRotation(m);
            Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));

            m = Matrix33F.CreateRotation(-Vector3F.UnitX, 1.3f);
            q = QuaternionF.CreateRotation(m);
            Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));

            m = Matrix33F.CreateRotation(-Vector3F.UnitY, -1.4f);
            q = QuaternionF.CreateRotation(m);
            Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));

            m = Matrix33F.CreateRotation(-Vector3F.UnitZ, -0.1f);
            q = QuaternionF.CreateRotation(m);
            Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));

            m = new Matrix33F(0, 0, 1,
                              0, -1, 0,
                              1, 0, 0);
            q = QuaternionF.CreateRotation(m);
            Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));

            m = new Matrix33F(-1, 0, 0,
                              0, 1, 0,
                              0, 0, -1);
            q = QuaternionF.CreateRotation(m);
            Assert.IsTrue(Vector3F.AreNumericallyEqual(m * v, q.Rotate(v)));
        }
Beispiel #19
0
        public void GetAxisAlignedBoundingBox()
        {
            Assert.AreEqual(new Aabb(), new Aabb().GetAabb(Pose.Identity));
            Assert.AreEqual(new Aabb(new Vector3F(10, 100, 1000), new Vector3F(10, 100, 1000)),
                            new Aabb().GetAabb(new Pose(new Vector3F(10, 100, 1000), QuaternionF.Identity)));
            Assert.AreEqual(new Aabb(new Vector3F(10, 100, 1000), new Vector3F(10, 100, 1000)),
                            new Aabb().GetAabb(new Pose(new Vector3F(10, 100, 1000), QuaternionF.CreateRotation(new Vector3F(1, 2, 3), 0.7f))));


            Aabb aabb = new Aabb(new Vector3F(1, 10, 100), new Vector3F(2, 20, 200));

            Assert.AreEqual(aabb, aabb.GetAabb(Pose.Identity));
            Assert.AreEqual(new Aabb(new Vector3F(11, 110, 1100), new Vector3F(12, 120, 1200)),
                            aabb.GetAabb(new Pose(new Vector3F(10, 100, 1000), QuaternionF.Identity)));
            // TODO: Test rotations.
        }
        // In this method, a vector is rotated with a quaternion and a matrix. The result
        // of the two vector rotations are compared.
        private void RotateVector()
        {
            var debugRenderer = GraphicsScreen.DebugRenderer2D;

            debugRenderer.DrawText("----- RotateVector Example:");

            // Create a vector. We will rotate this vector.
            Vector3F v = new Vector3F(1, 2, 3);

            // Create another vector which defines the axis of a rotation.
            Vector3F rotationAxis = Vector3F.UnitZ;

            // The rotation angle in radians. We want to rotate 50°.
            float rotationAngle = MathHelper.ToRadians(50);

            // ----- Part 1: Rotate a vector with a quaternion.

            // Create a quaternion that represents a 50° rotation around the axis given
            // by rotationAxis.
            QuaternionF rotation = QuaternionF.CreateRotation(rotationAxis, rotationAngle);

            // Rotate the vector v using the rotation quaternion.
            Vector3F vRotated = rotation.Rotate(v);

            // ----- Part 2: Rotate a vector with a matrix.

            // Create a matrix that represents a 50° rotation around the axis given by
            // rotationAxis.
            Matrix33F rotationMatrix = Matrix33F.CreateRotation(rotationAxis, rotationAngle);

            // Rotate the vector v using the rotation matrix.
            Vector3F vRotated2 = rotationMatrix * v;

            // ----- Part 3: Compare the results.
            // The result of both rotations should be identical.
            // Because of numerical errors there can be minor differences in the results.
            // Therefore we use Vector3F.AreNumericallyEqual() two check if the results
            // are equal (within a sensible numerical tolerance).
            if (Vector3F.AreNumericallyEqual(vRotated, vRotated2))
            {
                debugRenderer.DrawText("Vectors are equal.\n"); // This message is written.
            }
            else
            {
                debugRenderer.DrawText("Vectors are not equal.\n");
            }
        }
Beispiel #21
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>
        /// Called when <see cref="BoneMapper.MapAToB"/> was called.
        /// </summary>
        protected override void OnMapAToB()
        {
            CacheDerivedData();

            var skeletonInstanceA = SkeletonMapper.SkeletonPoseA;
            var skeletonInstanceB = SkeletonMapper.SkeletonPoseB;

            // ----- Set start transform.
            // Note: It is important to start from an absolute bone pose that does not depend
            // on the last frame. Otherwise, errors accumulate and the bone chain starts to twist.
            if (MapFromBindPose)
            {
                // Reset the bone rotation of the root bone to start from the bind pose.
                skeletonInstanceB.ResetBoneTransforms(RootBoneIndexB, RootBoneIndexB, false, true, false);
            }
            else
            {
                // Start from a direct-mapped bone pose.
                _directBoneMapper.MapAToB();
            }

            // Get absolute bone poses.
            var rootBoneA = skeletonInstanceA.GetBonePoseAbsolute(RootBoneIndexA);
            var tipBoneA  = skeletonInstanceA.GetBonePoseAbsolute(TipBoneIndexA);
            var rootBoneB = skeletonInstanceB.GetBonePoseAbsolute(RootBoneIndexB);
            var tipBoneB  = skeletonInstanceB.GetBonePoseAbsolute(TipBoneIndexB);

            // Compute direction vector for the two chains (origin to tip).
            var directionB = tipBoneB.Translation - rootBoneB.Translation;
            var directionA = tipBoneA.Translation - rootBoneA.Translation;

            // Abort if any chain has zero length.
            if (directionB.IsNumericallyZero || directionA.IsNumericallyZero)
            {
                return;
            }

            // Apply global skeleton rotation offset to rotate all into model B space.
            directionA = SkeletonMapper.RotationOffset.Rotate(directionA);

            // Compute and apply rotation between the two direction vectors.
            var rotation = QuaternionF.CreateRotation(directionB, directionA);

            skeletonInstanceB.RotateBoneAbsolute(RootBoneIndexB, rotation);
        }
        private void LimitBoneTransform()
        {
            // This method is called by the JacobianTransposeIKSolver after each internal iteration.
            // The job of this method is to apply bone limits; for example, the elbow should not
            // bend backwards, etc.
            // To apply a limit, get the bone transform or bone pose from skeleton pose, check if
            // is in the allowed range. If it is outside the allowed range, rotate it back to the
            // nearest allowed rotation.

            // Here, for example, we only make sure that the palm of the hand is always parallel
            // to the ground plane - as if the character wants to grab a horizontal bar or as
            // if it wants to place the hand on horizontal plane.
            SrtTransform bonePoseAbsolute = _meshNode.SkeletonPose.GetBonePoseAbsolute(15);
            Vector3F     palmAxis         = bonePoseAbsolute.ToParentDirection(-Vector3F.UnitY);

            bonePoseAbsolute.Rotation = QuaternionF.CreateRotation(palmAxis, Vector3F.UnitY) * bonePoseAbsolute.Rotation;
            _meshNode.SkeletonPose.SetBonePoseAbsolute(15, bonePoseAbsolute);
        }
        public void Division()
        {
            float       angle1 = 0.4f;
            Vector3F    axis1  = new Vector3F(1.0f, 2.0f, 3.0f);
            QuaternionF q1     = QuaternionF.CreateRotation(axis1, angle1);
            Matrix33F   m1     = Matrix33F.CreateRotation(axis1, angle1);

            float       angle2 = -1.6f;
            Vector3F    axis2  = new Vector3F(1.0f, -2.0f, -3.5f);
            QuaternionF q2     = QuaternionF.CreateRotation(axis2, angle2);
            Matrix33F   m2     = Matrix33F.CreateRotation(axis2, angle2);

            Vector3F v       = new Vector3F(0.3f, -2.4f, 5.6f);
            Vector3F result1 = QuaternionF.Divide(q2, q1).Rotate(v);
            Vector3F result2 = m2 * m1.Inverse * v;

            Assert.IsTrue(Vector3F.AreNumericallyEqual(result1, result2));
        }
        public void LerpQuaternionF()
        {
            // Warning: The not all results are not verified
            QuaternionF q1   = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f).Normalized;
            QuaternionF q2   = new QuaternionF(2.0f, 4.0f, 6.0f, 8.0f).Normalized;
            QuaternionF lerp = InterpolationHelper.Lerp(q1, q2, 0.75f);

            Assert.IsTrue(lerp.IsNumericallyNormalized);

            lerp = InterpolationHelper.Lerp(q1, q2, 0);
            Assert.IsTrue(QuaternionF.AreNumericallyEqual(q1, lerp));

            lerp = InterpolationHelper.Lerp(q1, q2, 1);
            Assert.IsTrue(QuaternionF.AreNumericallyEqual(q2, lerp));

            q1   = QuaternionF.Identity;
            q2   = QuaternionF.CreateRotation(Vector3F.UnitZ, (float)Math.PI / 2);
            lerp = InterpolationHelper.Lerp(q1, q2, 0.5f);
            Vector3F v      = lerp.Rotate(Vector3F.UnitX);
            Vector3F result = new Vector3F(1.0f, 1.0f, 0.0f).Normalized;

            Assert.IsTrue(Vector3F.AreNumericallyEqual(result, v));

            q1     = QuaternionF.Identity;
            q2     = QuaternionF.CreateRotation(Vector3F.UnitY, (float)Math.PI / 2);
            lerp   = InterpolationHelper.Lerp(q1, q2, 0.5f);
            v      = lerp.Rotate(Vector3F.UnitZ);
            result = new Vector3F(1.0f, 0.0f, 1.0f).Normalized;
            Assert.IsTrue(Vector3F.AreNumericallyEqual(result, v));

            q1     = QuaternionF.Identity;
            q2     = QuaternionF.CreateRotation(Vector3F.UnitX, (float)Math.PI / 2);
            lerp   = InterpolationHelper.Lerp(q1, q2, 0.5f);
            v      = lerp.Rotate(Vector3F.UnitY);
            result = new Vector3F(0.0f, 1.0f, 1.0f).Normalized;
            Assert.IsTrue(Vector3F.AreNumericallyEqual(result, v));

            q1     = new QuaternionF(-1.0f, 0.0f, 0.0f, 0.0f);
            q2     = QuaternionF.CreateRotation(-Vector3F.UnitZ, (float)-Math.PI / 2);
            lerp   = InterpolationHelper.Lerp(q1, q2, 0.5f);
            v      = lerp.Rotate(Vector3F.UnitX);
            result = new Vector3F(1.0f, 1.0f, 0.0f).Normalized;
            Assert.IsTrue(Vector3F.AreNumericallyEqual(result, v));
        }
Beispiel #26
0
        public void ComputeAngularVelocity()
        {
            var v  = new Vector3F(0.1f, 0.2f, 0.3f);
            var o0 = QuaternionF.CreateRotation(new Vector3F(-7, 8, 0.3f).Normalized, 4.2f);
            var dt = 1 / 60f;

            var o1 = (QuaternionF.CreateRotation(v, v.Length * dt) * o0).Normalized;

            // Negate quaternion. This is still the same rotation, but now we can test if
            // the rotation around the shortest arc is used.
            o1 = -o1;

            // We use a big epsilon. Quaternion multiplication seems to create a large error...?
            Assert.IsTrue(Vector3F.AreNumericallyEqual(v, AnimationHelper.ComputeAngularVelocity(o0, o1, dt), 0.01f));
            Assert.IsTrue(Vector3F.AreNumericallyEqual(v, AnimationHelper.ComputeAngularVelocity(o0.ToRotationMatrix33(), o1.ToRotationMatrix33(), dt), 0.01f));

            // Zero rotation.
            Assert.AreEqual(Vector3F.Zero, AnimationHelper.ComputeAngularVelocity(o0, -o0, dt));
        }
        public void Axis()
        {
            Vector3F axis  = new Vector3F(1.0f, 2.0f, 3.0f);
            float    angle = 0.2f;

            QuaternionF q = QuaternionF.CreateRotation(axis, angle);

            Assert.IsTrue(Numeric.AreEqual(angle, q.Angle));
            Assert.IsTrue(Vector3F.AreNumericallyEqual(axis.Normalized, q.Axis));
            axis   = new Vector3F(1.0f, 1.0f, 1.0f);
            q.Axis = axis;
            Assert.IsTrue(Numeric.AreEqual(angle, q.Angle));
            Assert.IsTrue(Vector3F.AreNumericallyEqual(axis.Normalized, q.Axis));
            Assert.IsTrue(Vector3F.AreNumericallyEqual(Matrix33F.CreateRotation(axis, angle) * Vector3F.One, q.Rotate(Vector3F.One)));

            Assert.AreEqual(Vector3F.Zero, QuaternionF.Identity.Axis);
            q.Axis = Vector3F.Zero;
            Assert.AreEqual(QuaternionF.Identity, q);
        }
        public void Inverse()
        {
            QuaternionF identity        = QuaternionF.Identity;
            QuaternionF inverseIdentity = identity.Inverse;

            Assert.AreEqual(inverseIdentity, identity);

            float    angle = 0.4f;
            Vector3F axis  = new Vector3F(1.0f, 1.0f, 1.0f);

            axis.Normalize();
            QuaternionF q       = QuaternionF.CreateRotation(axis, angle);
            QuaternionF inverse = q.Inverse;

            Assert.IsTrue(Vector3F.AreNumericallyEqual(-axis, inverse.Axis));

            q       = new QuaternionF(1, 2, 3, 4);
            inverse = q.Inverse;
            Assert.IsTrue(QuaternionF.AreNumericallyEqual(QuaternionF.Identity, inverse * q));
        }
        public void SlerpGeneralSinglePrecision()
        {
            QuaternionF q1    = QuaternionF.CreateRotation(-Vector3F.UnitY, (float)Math.PI / 2);
            QuaternionF q2    = QuaternionF.CreateRotation(Vector3F.UnitZ, (float)Math.PI / 2);
            QuaternionF slerp = InterpolationHelper.Slerp(q1, q2, 0.5f);

            Assert.IsTrue(slerp.IsNumericallyNormalized);
            Vector3F v      = slerp.Rotate(Vector3F.UnitX);
            Vector3F result = new Vector3F(1.0f / 3.0f, 2.0f / 3.0f, 2.0f / 3.0f); // I hope this is correct.

            Assert.IsTrue(Vector3F.AreNumericallyEqual(result, v));

            q1    = QuaternionF.CreateRotation(-Vector3F.UnitY, (float)Math.PI / 2);
            q2    = QuaternionF.CreateRotation(-Vector3F.UnitZ, (float)-Math.PI / 2);
            slerp = InterpolationHelper.Slerp(q1, q2, 0.5f);
            Assert.IsTrue(slerp.IsNumericallyNormalized);
            v      = slerp.Rotate(Vector3F.UnitX);
            result = new Vector3F(1.0f / 3.0f, 2.0f / 3.0f, 2.0f / 3.0f); // I hope this is correct.
            Assert.IsTrue(Vector3F.AreNumericallyEqual(result, v));
        }
Beispiel #30
0
        private static void InsertRandomKeyFrames(Random random, SrtKeyFrameAnimation animation, TimeSpan time0, TimeSpan time1,
                                                  float scaleThreshold, float rotationThreshold, float translationThreshold)
        {
            rotationThreshold = MathHelper.ToRadians(rotationThreshold);
            var defaultSource = SrtTransform.Identity;
            var defaultTarget = SrtTransform.Identity;
            var value         = new SrtTransform();

            int insertionIndex = 0;

            for (int i = 0; i < animation.KeyFrames.Count; i++)
            {
                if (animation.KeyFrames[i].Time == time0)
                {
                    insertionIndex = i + 1;
                    break;
                }
            }

            Debug.Assert(insertionIndex > 0);

            const int numberOfKeyFrames = 2;
            long      tickIncrement     = (time1 - time0).Ticks / (numberOfKeyFrames + 1);

            for (int i = 0; i < numberOfKeyFrames; i++)
            {
                var time = TimeSpan.FromTicks(time0.Ticks + (i + 1) * tickIncrement);
                Debug.Assert(time0 < time && time < time1);

                // Get interpolated animation value.
                animation.GetValue(time, ref defaultSource, ref defaultTarget, ref value);

                // Apply small variation (within thresholds).
                value.Scale       += random.NextVector3F(-1, 1).Normalized *(scaleThreshold / 2);
                value.Rotation     = QuaternionF.CreateRotation(random.NextVector3F(-1, 1), rotationThreshold / 2) * value.Rotation;
                value.Translation += random.NextVector3F(-1, 1).Normalized *(translationThreshold / 2);

                animation.KeyFrames.Insert(insertionIndex, new KeyFrame <SrtTransform>(time, value));
                insertionIndex++;
            }
        }