public void AdditionOperator() { QuaternionF a = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f); QuaternionF b = new QuaternionF(2.0f, 3.0f, 4.0f, 5.0f); QuaternionF c = a + b; Assert.AreEqual(new QuaternionF(3.0f, 5.0f, 7.0f, 9.0f), c); }
public void Addition() { QuaternionF a = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f); QuaternionF b = new QuaternionF(2.0f, 3.0f, 4.0f, 5.0f); QuaternionF c = QuaternionF.Add(a, b); Assert.AreEqual(new QuaternionF(3.0f, 5.0f, 7.0f, 9.0f), c); }
public void AreEqualWithEpsilon() { float epsilon = 0.001f; QuaternionF q1 = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f); QuaternionF q2 = new QuaternionF(1.002f, 2.002f, 3.002f, 4.002f); QuaternionF q3 = new QuaternionF(1.0001f, 2.0001f, 3.0001f, 4.0001f); Assert.IsTrue(QuaternionF.AreNumericallyEqual(q1, q1, epsilon)); Assert.IsFalse(QuaternionF.AreNumericallyEqual(q1, q2, epsilon)); Assert.IsTrue(QuaternionF.AreNumericallyEqual(q1, q3, epsilon)); }
public void AreEqual() { float originalEpsilon = Numeric.EpsilonF; Numeric.EpsilonF = 1e-8f; QuaternionF q1 = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f); QuaternionF q2 = new QuaternionF(1.000001f, 2.000001f, 3.000001f, 4.000001f); QuaternionF q3 = new QuaternionF(1.00000001f, 2.00000001f, 3.00000001f, 4.00000001f); Assert.IsTrue(QuaternionF.AreNumericallyEqual(q1, q1)); Assert.IsFalse(QuaternionF.AreNumericallyEqual(q1, q2)); Assert.IsTrue(QuaternionF.AreNumericallyEqual(q1, q3)); Numeric.EpsilonF = originalEpsilon; }
public void ConstructorTest() { var rotationQ = new QuaternionF(1, 2, 3, 4).Normalized; var srt = new SrtTransform(rotationQ.ToRotationMatrix33()); Assert.AreEqual(Vector3F.One, srt.Scale); Assert.IsTrue(QuaternionF.AreNumericallyEqual(rotationQ, srt.Rotation)); Assert.AreEqual(Vector3F.Zero, srt.Translation); srt = new SrtTransform(rotationQ); Assert.AreEqual(Vector3F.One, srt.Scale); Assert.IsTrue(QuaternionF.AreNumericallyEqual(rotationQ, srt.Rotation)); Assert.AreEqual(Vector3F.Zero, srt.Translation); srt = new SrtTransform(new Vector3F(-1, 2, -3), rotationQ.ToRotationMatrix33(), new Vector3F(10, 9, -8)); Assert.AreEqual(new Vector3F(-1, 2, -3), srt.Scale); Assert.IsTrue(QuaternionF.AreNumericallyEqual(rotationQ, srt.Rotation)); Assert.AreEqual(new Vector3F(10, 9, -8), srt.Translation); }
public static bool ApproximateEquals(this QuaternionF q0, QuaternionF q1) { return(ApproximateEquals(q0, q1, Constant <float> .PositiveTinyValue)); }
public BoneAnimationFrame(Bone bone, CoordinateF position, QuaternionF angles) { Bone = bone; Position = position; Angles = angles; }
// Handle character-related input and move the character. private void ControlCharacter(float deltaTime) { // Compute new orientation from mouse movement. float deltaYaw = -InputService.MousePositionDelta.X; _yaw += deltaYaw * deltaTime * 0.1f; float deltaPitch = -InputService.MousePositionDelta.Y; _pitch += deltaPitch * deltaTime * 0.1f; // Limit the pitch angle. _pitch = MathHelper.Clamp(_pitch, -ConstantsF.PiOver2, ConstantsF.PiOver2); // Compute new orientation of the camera. QuaternionF cameraOrientation = QuaternionF.CreateRotationY(_yaw) * QuaternionF.CreateRotationX(_pitch); // Create velocity from WASD keys. // TODO: Diagonal movement is faster ;-). Fix this. Vector3F velocityVector = Vector3F.Zero; if (Keyboard.GetState().IsKeyDown(Keys.W)) { velocityVector.Z--; } if (Keyboard.GetState().IsKeyDown(Keys.S)) { velocityVector.Z++; } if (Keyboard.GetState().IsKeyDown(Keys.A)) { velocityVector.X--; } if (Keyboard.GetState().IsKeyDown(Keys.D)) { velocityVector.X++; } velocityVector *= 10 * deltaTime; // Velocity vector is currently in view space. -z is the forward direction. // We have to convert this vector to world space by rotating it. velocityVector = QuaternionF.CreateRotationY(_yaw).Rotate(velocityVector); // New compute desired character controller position in world space: Vector3F targetPosition = _character.Position + velocityVector; // Check if user wants to jump. bool jump = Keyboard.GetState().IsKeyDown(Keys.Space); // Call character controller to compute a new valid position. The character // controller slides along obstacles, handles stepping up/down, etc. _character.Move(targetPosition, deltaTime, jump); // ----- Set view matrix for graphics. // For third person we move the eye position back, behind the body (+z direction is // the "back" direction). Vector3F thirdPersonDistance = cameraOrientation.Rotate(new Vector3F(0, 0, 6)); // Compute camera pose (= position + orientation). _cameraNode.PoseWorld = new Pose { Position = _character.Position // Floor position of character + new Vector3F(0, 1.6f, 0) // + Eye height + thirdPersonDistance, Orientation = cameraOrientation.ToRotationMatrix33() }; }
public void LnException() { QuaternionF q = new QuaternionF(1.5f, 0.0f, 0.0f, 0.0f); QuaternionF.Ln(q); }
public void GetValueTest() { var animation = new AnimationClip <float> { Animation = new SingleFromToByAnimation { From = 100, To = 200, Duration = TimeSpan.FromSeconds(6.0), }, Delay = TimeSpan.FromSeconds(10), Speed = 2, FillBehavior = FillBehavior.Hold, }; var animation2 = new AnimationClip <float> { Animation = new SingleFromToByAnimation { From = 10, To = 20, Duration = TimeSpan.FromSeconds(5.0), }, Delay = TimeSpan.FromSeconds(0), Speed = 1, FillBehavior = FillBehavior.Hold, }; var animation3 = new AnimationClip <float> { Animation = new SingleFromToByAnimation { From = 5, To = -5, Duration = TimeSpan.FromSeconds(10), }, Delay = TimeSpan.FromSeconds(5), Speed = 1, FillBehavior = FillBehavior.Hold, }; var animation4 = new AnimationClip <float> { Animation = new SingleFromToByAnimation { From = 1000, To = 1100, Duration = TimeSpan.FromSeconds(5.0), }, Delay = TimeSpan.FromSeconds(5), Speed = 1, FillBehavior = FillBehavior.Stop, }; var animationEx = new QuaternionFAnimation { W = animation, X = animation2, Y = animation3, Z = animation4, }; var defaultSource = new QuaternionF(1, 2, 3, 4); var defaultTarget = new QuaternionF(5, 6, 7, 8); var result = animationEx.GetValue(TimeSpan.FromSeconds(0.0), defaultSource, defaultTarget); Assert.AreEqual(defaultSource.W, result.W); // animation has not started. Assert.AreEqual(10.0f, result.X); // animation2 has started. Assert.AreEqual(defaultSource.Y, result.Y); // animation3 has not started. Assert.AreEqual(defaultSource.Z, result.Z); // animation4 has not started. result = animationEx.GetValue(TimeSpan.FromSeconds(5.0), defaultSource, defaultTarget); Assert.AreEqual(defaultSource.W, result.W); // animation has not started. Assert.AreEqual(20.0f, result.X); // animation2 has ended. Assert.AreEqual(5, result.Y); // animation3 has started. Assert.AreEqual(1000, result.Z); // animation4 has started. result = animationEx.GetValue(TimeSpan.FromSeconds(5.0), defaultSource, defaultTarget); Assert.AreEqual(defaultSource.W, result.W); // animation has not started. Assert.AreEqual(20.0f, result.X); // animation2 has ended. Assert.AreEqual(5, result.Y); // animation3 has started. Assert.AreEqual(1000, result.Z); // animation4 has started. result = animationEx.GetValue(TimeSpan.FromSeconds(13.0), defaultSource, defaultTarget); Assert.AreEqual(200, result.W); // animation has ended. Assert.AreEqual(20.0f, result.X); // animation2 is filling. Assert.AreEqual(-3, result.Y); // animation3 is active. Assert.AreEqual(defaultSource.Z, result.Z); // animation4 is stopped. }
/// <summary> /// Sets the bone rotation of a bone so that it matches the given rotation in model space. /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <param name="boneIndex">The index of the bone.</param> /// <param name="rotation">The rotation in model space.</param> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose" /> is <see langword="null"/>. /// </exception> public static void SetBoneRotationAbsolute(this SkeletonPose skeletonPose, int boneIndex, QuaternionF rotation) { if (skeletonPose == null) throw new ArgumentNullException("skeletonPose"); var skeleton = skeletonPose.Skeleton; var bindPoseRelative = skeleton.GetBindPoseRelative(boneIndex); var parentIndex = skeleton.GetParent(boneIndex); var parentBonePoseAbsolute = (parentIndex >= 0) ? skeletonPose.GetBonePoseAbsolute(parentIndex) : SrtTransform.Identity; // Solving this equation (using only rotations): // rotation = parentBonePoseAbsolute * bindPoseRelative * rotationRelative; // rotationRelative = boneTransform. var rotationRelative = bindPoseRelative.Rotation.Conjugated * parentBonePoseAbsolute.Rotation.Conjugated * rotation; rotationRelative.Normalize(); var boneTransform = skeletonPose.GetBoneTransform(boneIndex); boneTransform.Rotation = rotationRelative; skeletonPose.SetBoneTransform(boneIndex, boneTransform); }
public void Power2() { const float θ = 0.4f; const float t = -1.2f; Vector3F v = new Vector3F(2.3f, 1.0f, -2.0f); v.Normalize(); QuaternionF q = new QuaternionF((float)Math.Cos(θ), (float)Math.Sin(θ) * v); q.Power(t); QuaternionF expected = new QuaternionF((float)Math.Cos(t * θ), (float)Math.Sin(t * θ) * v); Assert.IsTrue(QuaternionF.AreNumericallyEqual(expected, q)); }
public ConstraintCarSample(Microsoft.Xna.Framework.Game game) : base(game) { // Add basic force effects. Simulation.ForceEffects.Add(new Gravity()); Simulation.ForceEffects.Add(new Damping()); // Create a material with high friction - this will be used for the ground and the wheels // to give the wheels some traction. UniformMaterial roughMaterial = new UniformMaterial { DynamicFriction = 1, StaticFriction = 1, }; // Add a ground plane. RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0), null, roughMaterial) { MotionType = MotionType.Static, }; Simulation.RigidBodies.Add(groundPlane); // Now, we build a car out of one box for the chassis and 4 cylindric wheels. // Front wheels are fixed with Hinge2Joints and motorized with AngularVelocityMotors. // Back wheels are fixed with HingeJoints and not motorized. - This creates a sloppy // car configuration. Please note that cars for racing games are not normally built with // simple constraints - nevertheless, it is funny to do and play with it. // Check out the "Vehicle Sample" (not included in this project)! The "Vehicle Sample" // provides a robust ray-car implementation.) // ----- Chassis BoxShape chassisShape = new BoxShape(1.7f, 1, 4f); MassFrame chassisMass = MassFrame.FromShapeAndDensity(chassisShape, Vector3F.One, 200, 0.01f, 3); // Here is a trick: The car topples over very easily. By lowering the center of mass we // make it more stable. chassisMass.Pose = new Pose(new Vector3F(0, -1, 0)); RigidBody chassis = new RigidBody(chassisShape, chassisMass, null) { Pose = new Pose(new Vector3F(0, 1, 0)), }; Simulation.RigidBodies.Add(chassis); // ------ Wheels CylinderShape cylinderShape = new CylinderShape(0.4f, 0.3f); MassFrame wheelMass = MassFrame.FromShapeAndDensity(cylinderShape, Vector3F.One, 500, 0.01f, 3); RigidBody wheelFrontLeft = new RigidBody(cylinderShape, wheelMass, roughMaterial) { Pose = new Pose(new Vector3F(0, 1, 0), Matrix33F.CreateRotationZ(ConstantsF.PiOver2)), }; Simulation.RigidBodies.Add(wheelFrontLeft); RigidBody wheelFrontRight = new RigidBody(cylinderShape, wheelMass, roughMaterial) { Pose = new Pose(new Vector3F(0, 1, 0), Matrix33F.CreateRotationZ(ConstantsF.PiOver2)), }; Simulation.RigidBodies.Add(wheelFrontRight); RigidBody wheelBackLeft = new RigidBody(cylinderShape, wheelMass, roughMaterial) { Pose = new Pose(new Vector3F(0, 1, 0), Matrix33F.CreateRotationZ(ConstantsF.PiOver2)), }; Simulation.RigidBodies.Add(wheelBackLeft); RigidBody wheelBackRight = new RigidBody(cylinderShape, wheelMass, roughMaterial) { Pose = new Pose(new Vector3F(0, 1, 0), Matrix33F.CreateRotationZ(ConstantsF.PiOver2)), }; Simulation.RigidBodies.Add(wheelBackRight); // ----- Hinge2Joints for the front wheels. // A Hinge2Joint allows a limited rotation on the first constraint axis - the steering // axis. The second constraint axis is locked and the third constraint axis is the // rotation axis of the wheels. _frontLeftHinge = new Hinge2Joint { BodyA = chassis, // --> To define the constraint anchor orientation for the chassis: // The columns are the axes. We set the local y axis in the first column. This is // steering axis. In the last column we set the -x axis. This is the wheel rotation axis. // In the middle column is a vector that is normal to the first and last axis. // (All three columns are orthonormal and form a valid rotation matrix.) AnchorPoseALocal = new Pose(new Vector3F(-0.9f, -0.4f, -1.4f), new Matrix33F(0, 0, -1, 1, 0, 0, 0, -1, 0)), BodyB = wheelFrontLeft, // --> To define the constraint anchor orientation for the chassis: // The columns are the axes. We set the local x axis in the first column. This is // steering axis. In the last column we set the y axis. This is the wheel rotation axis. // (In local space of a cylinder the cylinder axis is the +y axis.) // In the middle column is a vector that is normal to the first and last axis. // (All three columns are orthonormal and form a valid rotation matrix.) AnchorPoseBLocal = new Pose(new Matrix33F(1, 0, 0, 0, 0, 1, 0, -1, 0)), CollisionEnabled = false, Minimum = new Vector2F(-0.7f, float.NegativeInfinity), Maximum = new Vector2F(0.7f, float.PositiveInfinity), }; Simulation.Constraints.Add(_frontLeftHinge); _frontRightHinge = new Hinge2Joint { BodyA = chassis, BodyB = wheelFrontRight, AnchorPoseALocal = new Pose(new Vector3F(0.9f, -0.4f, -1.4f), new Matrix33F(0, 0, -1, 1, 0, 0, 0, -1, 0)), AnchorPoseBLocal = new Pose(new Matrix33F(1, 0, 0, 0, 0, 1, 0, -1, 0)), CollisionEnabled = false, Minimum = new Vector2F(-0.7f, float.NegativeInfinity), Maximum = new Vector2F(0.7f, float.PositiveInfinity), }; Simulation.Constraints.Add(_frontRightHinge); // ----- HingeJoints for the back wheels. // Hinges allow free rotation on the first constraint axis. HingeJoint backLeftHinge = new HingeJoint { BodyA = chassis, AnchorPoseALocal = new Pose(new Vector3F(-0.9f, -0.4f, 1.4f)), BodyB = wheelBackLeft, // --> To define the constraint anchor orientation: // The columns are the axes. We set the local y axis in the first column. This is // cylinder axis and should be the hinge axis. In the other two columns we set two // orthonormal vectors. // (All three columns are orthonormal and form a valid rotation matrix.) AnchorPoseBLocal = new Pose(new Matrix33F(0, 0, 1, 1, 0, 0, 0, 1, 0)), CollisionEnabled = false, }; Simulation.Constraints.Add(backLeftHinge); HingeJoint backRightHinge = new HingeJoint { BodyA = chassis, AnchorPoseALocal = new Pose(new Vector3F(0.9f, -0.4f, 1.4f)), BodyB = wheelBackRight, AnchorPoseBLocal = new Pose(new Matrix33F(0, 0, 1, 1, 0, 0, 0, 1, 0)), CollisionEnabled = false, }; Simulation.Constraints.Add(backRightHinge); // ----- Motors for the front wheels. // (Motor axes and target velocities are set in Update() below.) _frontLeftMotor = new AngularVelocityMotor { BodyA = chassis, BodyB = wheelFrontLeft, CollisionEnabled = false, // We use "single axis mode", which means the motor drives only on axis and does not // block motion orthogonal to this axis. - Rotation about the orthogonal axes is already // controlled by the Hinge2Joint. UseSingleAxisMode = true, // The motor has only limited power: MaxForce = 50000, }; Simulation.Constraints.Add(_frontLeftMotor); _frontRightMotor = new AngularVelocityMotor { BodyA = chassis, BodyB = wheelFrontRight, CollisionEnabled = false, UseSingleAxisMode = true, MaxForce = 50000, }; Simulation.Constraints.Add(_frontRightMotor); // ----- Drop a few boxes to create obstacles. BoxShape boxShape = new BoxShape(1, 1, 1); MassFrame boxMass = MassFrame.FromShapeAndDensity(boxShape, Vector3F.One, 100, 0.01f, 3); for (int i = 0; i < 20; i++) { Vector3F position = RandomHelper.Random.NextVector3F(-20, 20); position.Y = 5; QuaternionF orientation = RandomHelper.Random.NextQuaternionF(); RigidBody body = new RigidBody(boxShape, boxMass, null) { Pose = new Pose(position, orientation), }; Simulation.RigidBodies.Add(body); } }
public void NegationOperator() { QuaternionF a = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f); Assert.AreEqual(new QuaternionF(-1.0f, -2.0f, -3.0f, -4.0f), -a); }
public void Normalized() { QuaternionF q = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f); Assert.AreNotEqual(1.0f, q.Modulus); Assert.IsFalse(q.IsNumericallyNormalized); QuaternionF normalized = q.Normalized; Assert.AreEqual(new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f), q); Assert.IsTrue(Numeric.AreEqual(1.0f, normalized.Modulus)); Assert.IsTrue(normalized.IsNumericallyNormalized); }
public void Negation() { QuaternionF a = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f); Assert.AreEqual(new QuaternionF(-1.0f, -2.0f, -3.0f, -4.0f), QuaternionF.Negate(a)); }
public void MultiplyScalarOperator() { float s = 123.456f; QuaternionF q = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f); QuaternionF expectedResult = new QuaternionF(s * 1.0f, s * 2.0f, s * 3.0f, s * 4.0f); QuaternionF result1 = s * q; QuaternionF result2 = q * s; Assert.AreEqual(expectedResult, result1); Assert.AreEqual(expectedResult, result2); }
public void MultiplyScalar() { float s = 123.456f; QuaternionF q = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f); QuaternionF expectedResult = new QuaternionF(s * 1.0f, s * 2.0f, s * 3.0f, s * 4.0f); QuaternionF result = QuaternionF.Multiply(s, q); Assert.AreEqual(expectedResult, result); }
/// <summary> /// Called when <see cref="IKSolver.Solve"/> is called. /// </summary> /// <param name="deltaTime">The current time step (in seconds).</param> protected override void OnSolve(float deltaTime) { if (_isDirty) { // Validate bone chain. if (!SkeletonPose.IsAncestorOrSelf(RootBoneIndex, HingeBoneIndex)) { throw new ArgumentException("The RootBoneIndex and the HingeBoneIndex do not form a valid bone chain."); } if (!SkeletonPose.IsAncestorOrSelf(HingeBoneIndex, TipBoneIndex)) { throw new ArgumentException("The HingeBoneIndex and the TipBoneIndex do not form a valid bone chain."); } } _isDirty = false; if (MinHingeAngle > MaxHingeAngle) { throw new AnimationException("The MinHingeAngle must be less than or equal to the MaxHingeAngle"); } // Remember original bone transforms for interpolation at the end. var originalRootBoneTransform = SkeletonPose.GetBoneTransform(RootBoneIndex); var originalHingeBoneTransform = SkeletonPose.GetBoneTransform(HingeBoneIndex); var originalTipBoneTransform = SkeletonPose.GetBoneTransform(TipBoneIndex); var rootBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(RootBoneIndex); var hingeBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(HingeBoneIndex); var tipBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex); // Get tip position in model space. Vector3F tipAbsolute; if (TipBoneOrientation != null) { // If the user has specified an absolute tip rotation, then we consider this rotation and // use the tip bone origin as tip. tipAbsolute = tipBonePoseAbsolute.Translation; Target -= tipBonePoseAbsolute.ToParentDirection(tipBonePoseAbsolute.Scale * TipOffset); } else { // The user hasn't specified a desired tip rotation. Therefore we do not modify the // tip rotation and use the offset position in the tip bone as tip. tipAbsolute = tipBonePoseAbsolute.ToParentPosition(TipOffset); } // Abort if we already touch the target. if (Vector3F.AreNumericallyEqual(tipAbsolute, Target)) { return; } // Root to target vector. var rootToTarget = Target - rootBonePoseAbsolute.Translation; var rootToTargetLength = rootToTarget.Length; if (Numeric.IsZero(rootToTargetLength)) { return; } rootToTarget /= rootToTargetLength; // ----- Align chain with target. // Align the root to target vector with the root to tip vector. var rootToTip = tipAbsolute - rootBonePoseAbsolute.Translation; var rootToTipLength = rootToTip.Length; if (!Numeric.IsZero(rootToTipLength)) { rootToTip /= rootToTipLength; var rotation = QuaternionF.CreateRotation(rootToTip, rootToTarget); if (rotation.Angle > Numeric.EpsilonF) { // Apply rotation to root bone. rootBonePoseAbsolute.Rotation = rotation * rootBonePoseAbsolute.Rotation; SkeletonPose.SetBoneRotationAbsolute(RootBoneIndex, rootBonePoseAbsolute.Rotation); hingeBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(HingeBoneIndex); // Compute new tip absolute tip position from the known quantities. tipAbsolute = rootBonePoseAbsolute.Translation + rootToTarget * rootToTipLength; } } // ----- Compute ideal angle. var rootToHinge = hingeBonePoseAbsolute.Translation - rootBonePoseAbsolute.Translation; var hingeToTip = tipAbsolute - hingeBonePoseAbsolute.Translation; var hingeAxis = hingeBonePoseAbsolute.ToParentDirection(HingeAxis); // Project vectors to hinge plane. Everything should be in a plane for the following // computations. rootToHinge -= hingeAxis * Vector3F.Dot(rootToHinge, hingeAxis); hingeToTip -= hingeAxis * Vector3F.Dot(hingeToTip, hingeAxis); // Get lengths. float rootToHingeLength = rootToHinge.Length; if (Numeric.IsZero(rootToHingeLength)) { return; } rootToHinge /= rootToHingeLength; float hingeToTipLength = hingeToTip.Length; if (Numeric.IsZero(hingeToTipLength)) { return; } hingeToTip /= hingeToTipLength; // Compute current hinge angle (angle between root bone and hinge bone). float currentHingeAngle = (float)Math.Acos(MathHelper.Clamp(Vector3F.Dot(rootToHinge, hingeToTip), -1, 1)); // Make sure the computed angle is about the hingeAxis and not about -hingeAxis. if (Vector3F.Dot(Vector3F.Cross(rootToHinge, hingeToTip), hingeAxis) < 0) { currentHingeAngle = -currentHingeAngle; } // Using law of cosines to compute the desired hinge angle using the triangle lengths. float cosDesiredHingeAngle = (rootToHingeLength * rootToHingeLength + hingeToTipLength * hingeToTipLength - rootToTargetLength * rootToTargetLength) / (2 * rootToHingeLength * hingeToTipLength); float desiredHingeAngle = ConstantsF.Pi - (float)Math.Acos(MathHelper.Clamp(cosDesiredHingeAngle, -1, 1)); // Apply hinge limits. if (desiredHingeAngle < MinHingeAngle) { desiredHingeAngle = MinHingeAngle; } else if (desiredHingeAngle > MaxHingeAngle) { desiredHingeAngle = MaxHingeAngle; } // Compute delta rotation between current and desired angle. float deltaAngle = desiredHingeAngle - currentHingeAngle; var hingeRotation = QuaternionF.CreateRotation(hingeAxis, deltaAngle); hingeBonePoseAbsolute.Rotation = hingeRotation * hingeBonePoseAbsolute.Rotation; // Update tip position. tipAbsolute = hingeBonePoseAbsolute.Translation + hingeRotation.Rotate(tipAbsolute - hingeBonePoseAbsolute.Translation); // ----- Align chain with target. // If we hit a hinge limit, then we can move the tip closer to the target by aligning // the whole chain again. rootToTip = tipAbsolute - rootBonePoseAbsolute.Translation; rootToTipLength = rootToTip.Length; if (!Numeric.IsZero(rootToTipLength)) { rootToTip /= rootToTipLength; var rotation = QuaternionF.CreateRotation(rootToTip, rootToTarget); rootBonePoseAbsolute.Rotation = rotation * rootBonePoseAbsolute.Rotation; hingeBonePoseAbsolute.Rotation = rotation * hingeBonePoseAbsolute.Rotation; } // ----- Set results. SkeletonPose.SetBoneRotationAbsolute(RootBoneIndex, rootBonePoseAbsolute.Rotation); SkeletonPose.SetBoneRotationAbsolute(HingeBoneIndex, hingeBonePoseAbsolute.Rotation); if (TipBoneOrientation != null) { SkeletonPose.SetBoneRotationAbsolute(TipBoneIndex, TipBoneOrientation.Value); } // ----- Apply weight, velocity limit and set results. bool requiresBlending = RequiresBlending(); float maxRotationAngle; bool requiresLimiting = RequiresLimiting(deltaTime, out maxRotationAngle); if (requiresBlending || requiresLimiting) { var targetBoneTransform = SkeletonPose.GetBoneTransform(RootBoneIndex); if (requiresBlending) { BlendBoneTransform(ref originalRootBoneTransform, ref targetBoneTransform); } if (requiresLimiting) { LimitBoneTransform(ref originalRootBoneTransform, ref targetBoneTransform, maxRotationAngle); } SkeletonPose.SetBoneTransform(RootBoneIndex, targetBoneTransform); targetBoneTransform = SkeletonPose.GetBoneTransform(HingeBoneIndex); if (requiresBlending) { BlendBoneTransform(ref originalHingeBoneTransform, ref targetBoneTransform); } if (requiresLimiting) { LimitBoneTransform(ref originalHingeBoneTransform, ref targetBoneTransform, maxRotationAngle); } SkeletonPose.SetBoneTransform(HingeBoneIndex, targetBoneTransform); targetBoneTransform = SkeletonPose.GetBoneTransform(TipBoneIndex); if (requiresBlending) { BlendBoneTransform(ref originalTipBoneTransform, ref targetBoneTransform); } if (requiresLimiting) { LimitBoneTransform(ref originalTipBoneTransform, ref targetBoneTransform, maxRotationAngle); } SkeletonPose.SetBoneTransform(TipBoneIndex, targetBoneTransform); } }
public void Power3() { const float θ = 0.4f; Vector3F v = new Vector3F(2.3f, 1.0f, -2.0f); v.Normalize(); QuaternionF q = new QuaternionF((float)Math.Cos(θ), (float)Math.Sin(θ) * v); QuaternionF q2 = q; q2.Power(2); Assert.IsTrue(QuaternionF.AreNumericallyEqual(q * q, q2)); QuaternionF q3 = q; q3.Power(3); Assert.IsTrue(QuaternionF.AreNumericallyEqual(q * q * q, q3)); q2 = q; q2.Power(-2); Assert.IsTrue(QuaternionF.AreNumericallyEqual(q.Inverse * q.Inverse, q2)); q3 = q; q3.Power(-3); Assert.IsTrue(QuaternionF.AreNumericallyEqual(q.Inverse * q.Inverse * q.Inverse, q3)); }
protected override void OnUpdate(TimeSpan deltaTime) { // Mouse centering (controlled by the MenuComponent) is disabled if the game // is inactive or if the GUI is active. In these cases, we do not want to move // the player. if (!_inputService.EnableMouseCentering) { return; } float deltaTimeF = (float)deltaTime.TotalSeconds; // ----- Hulk Mode // Toggle "Hulk" mode if <H> or <X> (gamepad) is pressed. if (_inputService.IsPressed(Keys.H, false) || _inputService.IsPressed(Buttons.X, false, LogicalPlayerIndex.One)) { ToggleHulk(); } // ----- Crouching if (_inputService.IsPressed(Keys.LeftShift, false) || _inputService.IsPressed(Buttons.RightTrigger, false, LogicalPlayerIndex.One)) { Crouch(); } else if (!_inputService.IsDown(Keys.LeftShift) && !_inputService.IsDown(Buttons.RightTrigger, LogicalPlayerIndex.One) && CharacterController.Height <= 1) { StandUp(); } // ----- Update orientation // Update _yaw and _pitch. UpdateOrientation(deltaTimeF); // Compute the new orientation of the camera. QuaternionF orientation = QuaternionF.CreateRotationY(_yaw) * QuaternionF.CreateRotationX(_pitch); // ----- Compute translation // Create velocity from <W>, <A>, <S>, <D> and gamepad sticks. Vector3F moveDirection = Vector3F.Zero; if (Keyboard.GetState().IsKeyDown(Keys.W)) { moveDirection.Z--; } if (Keyboard.GetState().IsKeyDown(Keys.S)) { moveDirection.Z++; } if (Keyboard.GetState().IsKeyDown(Keys.A)) { moveDirection.X--; } if (Keyboard.GetState().IsKeyDown(Keys.D)) { moveDirection.X++; } moveDirection.X += _inputService.GetGamePadState(LogicalPlayerIndex.One).ThumbSticks.Left.X; moveDirection.Z -= _inputService.GetGamePadState(LogicalPlayerIndex.One).ThumbSticks.Left.Y; // Rotate the velocity vector from view space to world space. moveDirection = orientation.Rotate(moveDirection); // Add velocity from <R>, <F> keys. // <R> or DPad up is used to move up ("rise"). // <F> or DPad down is used to move down ("fall"). if (Keyboard.GetState().IsKeyDown(Keys.R) || _inputService.GetGamePadState(LogicalPlayerIndex.One).DPad.Up == ButtonState.Pressed) { moveDirection.Y++; } if (Keyboard.GetState().IsKeyDown(Keys.F) || _inputService.GetGamePadState(LogicalPlayerIndex.One).DPad.Down == ButtonState.Pressed) { moveDirection.Y--; } // ----- Climbing bool hasLadderContact = HasLadderContact(); bool hasLedgeContact = HasLedgeContact(); CharacterController.IsClimbing = hasLadderContact || hasLedgeContact; // When the character is walking (gravity > 0) it cannot walk up/down - only on a ladder. if (CharacterController.Gravity != 0 && !hasLadderContact) { moveDirection.Y = 0; } // ----- Moving moveDirection.TryNormalize(); Vector3F moveVelocity = moveDirection * LinearVelocityMagnitude; // ----- Jumping if ((_inputService.IsPressed(Keys.Space, false) || _inputService.IsPressed(Buttons.A, false, LogicalPlayerIndex.One)) && (CharacterController.HasGroundContact || CharacterController.IsClimbing)) { // Jump button was newly pressed and the character has support to start the jump. _timeSinceLastJump = 0; } float jumpVelocity = 0; if ((_inputService.IsDown(Keys.Space) || _inputService.IsDown(Buttons.A, LogicalPlayerIndex.One))) { // Jump button is still down. if (_timeSinceLastJump + deltaTimeF <= DynamicJumpTime) { // The DynamicJumpTime has not been exceeded. // Set a jump velocity to make the jump higher. jumpVelocity = JumpVelocity; } else if (_timeSinceLastJump <= DynamicJumpTime) { // The jump time exceeds DynamicJumpTime in this time step. // _timeSinceLastJump <= DynamicJumpTime // _timeSinceLastJump + deltaTime > DynamicJumpTime // In order to achieve exact, reproducible jump heights we need to split // the time step: float deltaTime0 = DynamicJumpTime - _timeSinceLastJump; float deltaTime1 = deltaTimeF - deltaTime0; // The first part of the movement is a jump with active jump velocity. jumpVelocity = JumpVelocity; _timeSinceLastJump += deltaTime0; CharacterController.Move(moveVelocity, jumpVelocity, deltaTime0); // The second part of the movement is a jump without jump velocity. jumpVelocity = 0; deltaTimeF = deltaTime1; } } _timeSinceLastJump += deltaTimeF; // ----- Move the character. CharacterController.Move(moveVelocity, jumpVelocity, deltaTimeF); // Draw character controller capsule. // The character controller is also transparent The hulk is green, of course. var color = _isHulk ? Color.DarkGreen : Color.Gray; color.A = 128; _debugRenderer.DrawObject(CharacterController.Body, color, false, false); }
public void Properties() { QuaternionF q = new QuaternionF(0.123f, 1.0f, 2.0f, 3.0f); Assert.AreEqual(0.123f, q.W); Assert.AreEqual(1.0f, q.X); Assert.AreEqual(2.0f, q.Y); Assert.AreEqual(3.0f, q.Z); q.W = 1.0f; q.X = 2.0f; q.Y = 3.0f; q.Z = 4.0f; Assert.AreEqual(1.0f, q.W); Assert.AreEqual(2.0f, q.X); Assert.AreEqual(3.0f, q.Y); Assert.AreEqual(4.0f, q.Z); q.V = new Vector3F(-1.0f, -2.0f, -3.0f); Assert.AreEqual(-1.0f, q.X); Assert.AreEqual(-2.0f, q.Y); Assert.AreEqual(-3.0f, q.Z); Assert.AreEqual(new Vector3F(-1.0f, -2.0f, -3.0f), q.V); }
//public static void RotateBoneWorld(this SkeletonPose SkeletonPose, int boneIndex, QuaternionF rotation, Matrix44F world) //{ // QuaternionF worldRotation = QuaternionF.CreateRotation(world.Minor); // RotateBoneAbsolute(SkeletonPose, boneIndex, worldRotation.Conjugated * rotation); //} // TODO: This method should really be called RotateBoneLocalAnimated? ///// <summary> ///// Rotates bone where the rotation is given in the bone space. ///// </summary> ///// <param name="skeletonPose">The skeleton pose.</param> ///// <param name="boneIndex">The index of the bone.</param> ///// <param name="rotation">The rotation in bone space.</param> ///// <exception cref="ArgumentNullException"> ///// <paramref name="skeletonPose" /> is <see langword="null"/>. ///// </exception> //public static void RotateBoneLocal(this SkeletonPose skeletonPose, int boneIndex, QuaternionF rotation) //{ // if (skeletonPose == null) // throw new ArgumentNullException("skeletonPose"); // var boneTransform = skeletonPose.GetBoneTransform(boneIndex); // boneTransform.Rotation = boneTransform.Rotation * rotation; // skeletonPose.SetBoneTransform(boneIndex, boneTransform); //} /// <summary> /// Rotates a bone where the rotation is given in model space. /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <param name="boneIndex">The index of the bone.</param> /// <param name="rotation">The rotation in model space.</param> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose" /> is <see langword="null"/>. /// </exception> public static void RotateBoneAbsolute(this SkeletonPose skeletonPose, int boneIndex, QuaternionF rotation) { if (skeletonPose == null) { throw new ArgumentNullException("skeletonPose"); } var skeleton = skeletonPose.Skeleton; var boneTransform = skeletonPose.GetBoneTransform(boneIndex); var bindPoseRelative = skeleton.GetBindPoseRelative(boneIndex); var parentIndex = skeleton.GetParent(boneIndex); var parentBonePoseAbsolute = skeletonPose.GetBonePoseAbsolute(parentIndex); // Solving these equations (using only the rotations): // rotation * bonePoseAbsolute = bonePoseAbsoluteNew // bonePoseAbsolute = parentBonePoseAbsolute * bindPoseRelative * boneTransform. // ... // Rotation relative to bone bind pose space (using similarity transformation). var rotationRelative = bindPoseRelative.Rotation.Conjugated * parentBonePoseAbsolute.Rotation.Conjugated * rotation * parentBonePoseAbsolute.Rotation * bindPoseRelative.Rotation; // The final rotation is: First rotate into bone bind pose space, then apply rotation. boneTransform.Rotation = rotationRelative * boneTransform.Rotation; // So many multiplications, numerical errors adds up quickly in iterative IK algorithms... boneTransform.Rotation.Normalize(); skeletonPose.SetBoneTransform(boneIndex, boneTransform); }
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); }
protected static double CalculateYaw(QuaternionF orientation) { //Contract.Requires<ArgumentNullException>(orientation != null, "orientation"); return(System.Math.Atan2(2.0f * (orientation.W * orientation.Z + orientation.X * orientation.Y), 1.0f - 2.0f * (orientation.Y * orientation.Y + orientation.Z * orientation.Z))); }
public void Update(TimeSpan deltaTime) { // Update the position based upon keyboard _orientation = QuaternionF.CreateRotationY(_yaw) * QuaternionF.CreateRotationX(_pitch); _position.X = MathHelper.Clamp(_position.X, -170.0f, 170.0f); _position.Y = MathHelper.Clamp(_position.Y, 40.0f, 200.0f); _position.Z = MathHelper.Clamp(_position.Z, -170.0f, 170.0f); _cameraNode.PoseWorld = new Pose(_position, _orientation); }
private void OnXyzwChanged() { if (_isUpdating) return; _isUpdating = true; try { if (_vectorType == typeof(Vector4)) Value = new Vector4((float)X, (float)Y, (float)Z, (float)W); else if (_vectorType == typeof(Quaternion)) Value = new Quaternion((float)X, (float)Y, (float)Z, (float)W); else if (_vectorType == typeof(Vector4F)) Value = new Vector4F((float)X, (float)Y, (float)Z, (float)W); else if (_vectorType == typeof(Vector4D)) Value = new Vector4D(X, Y, Z, W); else if (_vectorType == typeof(QuaternionF)) Value = new QuaternionF((float)W, (float)X, (float)Y, (float)Z); else if (_vectorType == typeof(QuaternionD)) Value = new QuaternionD(W, X, Y, Z); } finally { _isUpdating = false; } }
public void GetValueTest() { var animation = new AnimationClip<float> { Animation = new SingleFromToByAnimation { From = 100, To = 200, Duration = TimeSpan.FromSeconds(6.0), }, Delay = TimeSpan.FromSeconds(10), Speed = 2, FillBehavior = FillBehavior.Hold, }; var animation2 = new AnimationClip<float> { Animation = new SingleFromToByAnimation { From = 10, To = 20, Duration = TimeSpan.FromSeconds(5.0), }, Delay = TimeSpan.FromSeconds(0), Speed = 1, FillBehavior = FillBehavior.Hold, }; var animation3 = new AnimationClip<float> { Animation = new SingleFromToByAnimation { From = 5, To = -5, Duration = TimeSpan.FromSeconds(10), }, Delay = TimeSpan.FromSeconds(5), Speed = 1, FillBehavior = FillBehavior.Hold, }; var animation4 = new AnimationClip<float> { Animation = new SingleFromToByAnimation { From = 1000, To = 1100, Duration = TimeSpan.FromSeconds(5.0), }, Delay = TimeSpan.FromSeconds(5), Speed = 1, FillBehavior = FillBehavior.Stop, }; var animationEx = new QuaternionFAnimation { W = animation, X = animation2, Y = animation3, Z = animation4, }; var defaultSource = new QuaternionF(1, 2, 3, 4); var defaultTarget = new QuaternionF(5, 6, 7, 8); var result = animationEx.GetValue(TimeSpan.FromSeconds(0.0), defaultSource, defaultTarget); Assert.AreEqual(defaultSource.W, result.W); // animation has not started. Assert.AreEqual(10.0f, result.X); // animation2 has started. Assert.AreEqual(defaultSource.Y, result.Y); // animation3 has not started. Assert.AreEqual(defaultSource.Z, result.Z); // animation4 has not started. result = animationEx.GetValue(TimeSpan.FromSeconds(5.0), defaultSource, defaultTarget); Assert.AreEqual(defaultSource.W, result.W); // animation has not started. Assert.AreEqual(20.0f, result.X); // animation2 has ended. Assert.AreEqual(5, result.Y); // animation3 has started. Assert.AreEqual(1000, result.Z); // animation4 has started. result = animationEx.GetValue(TimeSpan.FromSeconds(5.0), defaultSource, defaultTarget); Assert.AreEqual(defaultSource.W, result.W); // animation has not started. Assert.AreEqual(20.0f, result.X); // animation2 has ended. Assert.AreEqual(5, result.Y); // animation3 has started. Assert.AreEqual(1000, result.Z); // animation4 has started. result = animationEx.GetValue(TimeSpan.FromSeconds(13.0), defaultSource, defaultTarget); Assert.AreEqual(200, result.W); // animation has ended. Assert.AreEqual(20.0f, result.X); // animation2 is filling. Assert.AreEqual(-3, result.Y); // animation3 is active. Assert.AreEqual(defaultSource.Z, result.Z); // animation4 is stopped. }
public void FromToMatrixTest() { var t = new Vector3F(1, 2, 3); var r = new QuaternionF(1, 2, 3, 4).Normalized; var s = new Vector3F(2, 7, 9); var m = Matrix44F.CreateTranslation(t) * Matrix44F.CreateRotation(r) * Matrix44F.CreateScale(s); var srt = SrtTransform.FromMatrix(m); Assert.IsTrue(Vector3F.AreNumericallyEqual(t, srt.Translation)); Assert.IsTrue(QuaternionF.AreNumericallyEqual(r, srt.Rotation)); Assert.IsTrue(Vector3F.AreNumericallyEqual(s, srt.Scale)); // XNA: srt = SrtTransform.FromMatrix((Matrix)m); Assert.IsTrue(Vector3F.AreNumericallyEqual(t, srt.Translation)); Assert.IsTrue(QuaternionF.AreNumericallyEqual(r, srt.Rotation)); Assert.IsTrue(Vector3F.AreNumericallyEqual(s, srt.Scale)); // With negative scale, the decomposition is not unique (many possible combinations of // axis mirroring + rotation). t = new Vector3F(1, 2, 3); r = new QuaternionF(1, 2, 3, 4).Normalized; s = new Vector3F(2, -7, 9); m = Matrix44F.CreateTranslation(t) * Matrix44F.CreateRotation(r) * Matrix44F.CreateScale(s); srt = SrtTransform.FromMatrix(m); var m2 = (Matrix44F)srt; Assert.IsTrue(Matrix44F.AreNumericallyEqual(m, m2)); m2 = srt.ToMatrix44F(); Assert.IsTrue(Matrix44F.AreNumericallyEqual(m, m2)); m2 = srt; Assert.IsTrue(Matrix44F.AreNumericallyEqual(m, m2)); Matrix mXna = srt.ToXna(); Assert.IsTrue(Matrix44F.AreNumericallyEqual(m, (Matrix44F)mXna)); mXna = srt; Assert.IsTrue(Matrix44F.AreNumericallyEqual(m, (Matrix44F)mXna)); }
/// <summary> /// Perform mapping in absolute space. /// </summary> private static void MapAbsolute(bool mapTranslations, SkeletonPose skeletonA, int boneIndexA, SkeletonPose skeletonB, int boneIndexB, float scaleAToB, QuaternionF rotationBToA, QuaternionF rotationAToB) { // The current absolute bone pose of bone A. var boneAActualRotationAbsolute = skeletonA.GetBonePoseAbsolute(boneIndexA).Rotation; var boneABindRotationAbsolute = skeletonA.Skeleton.GetBindPoseAbsoluteInverse(boneIndexA).Rotation; var boneBBindRotationAbsolute = skeletonB.Skeleton.GetBindPoseAbsoluteInverse(boneIndexB).Rotation.Inverse; var relativeRotation = boneAActualRotationAbsolute * boneABindRotationAbsolute; // Rotation: Using similarity transformation: (Read from right to left.) // Rotate from model B space to model A space. // Apply the bone transform rotation in model A space // Rotate back from model A space to model B space. relativeRotation = rotationAToB * relativeRotation * rotationBToA; skeletonB.SetBoneRotationAbsolute(boneIndexB, relativeRotation * boneBBindRotationAbsolute); // TODO: Map translations. // How? // Map translation relative to model space? // Map translation relative to the next common bone ancestor that is in both skeletons // (then we would need a mechanism or user input that gives us this ancestor, complicated)? // Map translation relative to local space (the bone transform translation as in MapLocal)? }
public static float Norm(QuaternionF q) => q.Norm;
/// <summary> /// Perform mapping in local bone space. /// </summary> private static void MapLocal(bool mapTranslations, SkeletonPose skeletonA, int boneIndexA, SkeletonPose skeletonB, int boneIndexB, float scaleAToB, QuaternionF rotationBToA, QuaternionF rotationAToB) { var boneTransform = skeletonA.GetBoneTransform(boneIndexA); // Remove any scaling. boneTransform.Scale = Vector3F.One; // Rotation: Using similarity transformation: (Read from right to left.) // Rotate from bone B space to bone A space. // Apply the bone transform rotation in bone A space // Rotate back from bone A space to bone B space. boneTransform.Rotation = rotationAToB * boneTransform.Rotation * rotationBToA; // If we should map translations, then we scale the translation and rotate it from // bone A space to bone B space. if (mapTranslations) boneTransform.Translation = rotationAToB.Rotate(boneTransform.Translation * scaleAToB); else boneTransform.Translation = Vector3F.Zero; // Apply new bone transform to B. skeletonB.SetBoneTransform(boneIndexB, boneTransform); }
public static bool ApproximateEquals(this QuaternionF q0, QuaternionF q1, float tolerance) { return(ApproximateEquals(q0.W, q1.W, tolerance) && ApproximateEquals(q0.X, q1.X, tolerance) && ApproximateEquals(q0.Y, q1.Y, tolerance) && ApproximateEquals(q0.Z, q1.Z, tolerance)); }
private void CacheDerivedData() { // Abort if cached data is still valid. if (!_isDirty) return; _isDirty = false; // Check bone indices. var skeletonInstanceA = SkeletonMapper.SkeletonPoseA; var skeletonInstanceB = SkeletonMapper.SkeletonPoseB; var skeletonA = skeletonInstanceA.Skeleton; var skeletonB = skeletonInstanceB.Skeleton; if (BoneIndexA < 0 || BoneIndexA >= skeletonInstanceA.Skeleton.NumberOfBones) throw new IndexOutOfRangeException("BoneIndexA is out of range."); if (BoneIndexB < 0 || BoneIndexB >= skeletonInstanceB.Skeleton.NumberOfBones) throw new IndexOutOfRangeException("BoneIndexB is out of range."); // Compute offsets. var bindPoseAAbsoluteInverse = skeletonA.GetBindPoseAbsoluteInverse(BoneIndexA); var bindPoseBAbsoluteInverse = skeletonB.GetBindPoseAbsoluteInverse(BoneIndexB); if (MapAbsoluteTransforms) { _rotationAToB = SkeletonMapper.RotationOffset; } else { // Read from right to left: // Change from A bone space to model A space. // Apply rotation offset to change from model A space to model B space. // Change model B space to B bone space. // --> The result transforms from bone A space to bone B space. _rotationAToB = bindPoseBAbsoluteInverse.Rotation * SkeletonMapper.RotationOffset * bindPoseAAbsoluteInverse.Rotation.Conjugated; } _rotationAToB.Normalize(); }
/// <summary> /// Called when <see cref="IKSolver.Solve"/> is called. /// </summary> /// <param name="deltaTime">The current time step (in seconds).</param> protected override void OnSolve(float deltaTime) { UpdateDerivedValues(); if (_totalChainLength == 0) { return; } var skeleton = SkeletonPose.Skeleton; // Make a local copy of the absolute bone poses. The following operations will be performed // on the data in _bone and not on the skeleton pose. var numberOfBones = _boneIndices.Count; for (int i = 0; i < numberOfBones; i++) { _bones[i] = SkeletonPose.GetBonePoseAbsolute(_boneIndices[i]); } // Calculate the position at the tip of the last bone. // If TipOffset is not 0, then we can rotate the last bone. // If TipOffset is 0, then the last bone defines the tip but is not rotated. // --> numberOfBones is set to the number of affected bones. Vector3F tipAbsolute; if (TipOffset.IsNumericallyZero) { numberOfBones--; tipAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex).Translation; } else { tipAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex).ToParentPosition(TipOffset); } // The root bone rotation that aligns the whole chain with the target. QuaternionF chainRotation = QuaternionF.Identity; Vector3F boneToTarget, boneToTip; float remainingChainLength = _totalChainLength; // Apply the soft limit to the distance to the IK goal //vecToIkGoal = Target - _bones[0].Translation; //distToIkGoal = vecToIkGoal.Length; //// Limit the extension to 98% and ramp it up over 5% of the chains length //vecToIkGoal *= (LimitValue(distToIkGoal, _totalChainLength * 0.98f, _totalChainLength * 0.08f)) / distToIkGoal; //Vector3F goalPosition = _bones[0].Translation + vecToIkGoal; var targetAbsolute = Target; // This algorithms iterates once over all bones from root to tip. for (int i = 0; i < numberOfBones; i++) { if (i > 0) { // Transform the bone position by the overall chain offset. var translation = _bones[0].Translation + chainRotation.Rotate(_bones[i].Translation - _bones[0].Translation); _bones[i] = new SrtTransform(_bones[i].Scale, _bones[i].Rotation, translation); } // The bone to tip vector of the aligned chain (without other IK rotations!). boneToTip = tipAbsolute - _bones[i].Translation; float boneToTipLength = boneToTip.Length; boneToTip /= boneToTipLength; // TODO: Check for division by 0? if (i > 0) { // Calculate the new absolute bone position. var translation = _bones[i - 1].ToParentPosition(skeleton.GetBindPoseRelative(_boneIndices[i]).Translation); _bones[i] = new SrtTransform(_bones[i].Scale, _bones[i].Rotation, translation); } // The bone to target vector of the new chain configuration. boneToTarget = targetAbsolute - _bones[i].Translation; float boneToTargetLength = boneToTarget.Length; boneToTarget /= boneToTargetLength; if (i == 0) { // This is the first bone: Compute rotation that aligns the whole initial chain with // the target. chainRotation = QuaternionF.CreateRotation(boneToTip, boneToTarget); // Update tip. tipAbsolute = _bones[i].Translation + (boneToTarget * boneToTipLength); // Apply chainRotation to root bone. _bones[i] = new SrtTransform(_bones[i].Scale, chainRotation * _bones[i].Rotation, _bones[i].Translation); } else { // Apply the chain alignment rotation. Also the parent bones have changed, so we apply // an additional rotation that accounts for the ancestor rotations. This additional // rotation aligns the last bone with the target. // TODO: Find an explanation/derivation of this additional rotation. _bones[i] = new SrtTransform( _bones[i].Scale, QuaternionF.CreateRotation(boneToTip, boneToTarget) * chainRotation * _bones[i].Rotation, _bones[i].Translation); } // Now, solve the bone using trigonometry. // The last bone was already aligned with the target. For the second last bone we use // the law of cosines. For all other bones we use the complicated steps described in the // GPG article. if (i <= numberOfBones - 2) { // Length of chain after this bone. remainingChainLength -= _boneLengths[i]; // The direction of the current bone. For the tip bone we use the TipOffset. Vector3F boneDirection; if (i != TipBoneIndex) { boneDirection = _bones[i].Rotation.Rotate(skeleton.GetBindPoseRelative(_boneIndices[i + 1]).Translation); } else { boneDirection = _bones[i].Rotation.Rotate(TipOffset); } if (!boneDirection.TryNormalize()) { continue; } // The bone rotates around an axis normal to the bone to target direction and the bone // vector. Vector3F rotationAxis = Vector3F.Cross(boneToTarget, boneDirection); if (!rotationAxis.TryNormalize()) { continue; // TODO: If this happens, can we choose a useful direction? } // The current angle between bone direction and bone to target vector. float currentAngle = (float)Math.Acos(MathHelper.Clamp(Vector3F.Dot(boneDirection, boneToTarget), -1, 1)); // Side lengths of the involved triangles. var a = _boneLengths[i]; var b = boneToTargetLength; var c = remainingChainLength; var d = boneToTipLength; float desiredAngle; if (i == numberOfBones - 2) { // Use trigonometry (law of cosines) to determine the desired angle. desiredAngle = (float)Math.Acos(MathHelper.Clamp((a * a + b * b - c * c) / (2 * a * b), -1.0f, 1.0f)); } else { // The maximal angle that this bone can have where the chain still reaches the tip. float maxTipAngle; if (boneToTipLength > remainingChainLength) { maxTipAngle = (float)Math.Acos(MathHelper.Clamp((a * a + d * d - c * c) / (2 * a * d), -1.0f, 1.0f)); } else { // Tip is very near and this bone can bend more than 180°. Add additional chain length // in radians. maxTipAngle = (float)Math.Acos(MathHelper.Clamp((a * 0.5f) / remainingChainLength, 0.0f, 1.0f)); maxTipAngle += ((c - d) / a); } // The maximal angle that this bone can have where the chain still reaches the target. float maxTargetAngle; if (boneToTargetLength > remainingChainLength) { maxTargetAngle = (float)Math.Acos(MathHelper.Clamp((a * a + b * b - c * c) / (2 * a * b), -1.0f, 1.0f)); } else { // Target is very near and this bone can bend more than 180°. Add additional chain // length in radians. maxTargetAngle = (float)Math.Acos(MathHelper.Clamp((a * 0.5f) / remainingChainLength, 0.0f, 1.0f)); maxTargetAngle += ((c - b) / a); } // If we set the desired angle to maxTargetAngle, the remain bones must be all // stretched. We want to keep the chain appearance, therefore, we set a smaller angle. // The new angle relative to the final remaining chain should have the same ratio as the // current angle to the current remaining chain. if (!Numeric.IsZero(maxTipAngle)) { desiredAngle = maxTargetAngle * (currentAngle / maxTipAngle); } else { desiredAngle = maxTargetAngle; // Avoiding divide by zero. } } // The rotation angle that we have to apply. float deltaAngle = desiredAngle - currentAngle; // Apply the rotation to the current bones _bones[i] = new SrtTransform( _bones[i].Scale, (QuaternionF.CreateRotation(rotationAxis, deltaAngle) * _bones[i].Rotation).Normalized, _bones[i].Translation); } } bool requiresBlending = RequiresBlending(); float maxRotationAngle; bool requiresLimiting = RequiresLimiting(deltaTime, out maxRotationAngle); if (requiresBlending || requiresLimiting) { // We have to blend the computed results with the original bone transforms. // Get original bone transforms. for (int i = 0; i < numberOfBones; i++) { _originalBoneTransforms.Add(SkeletonPose.GetBoneTransform(_boneIndices[i])); } for (int i = 0; i < numberOfBones; i++) { int boneIndex = _boneIndices[i]; var originalBoneTransform = _originalBoneTransforms[i]; // Set absolute bone pose and let the skeleton compute the bone transform for us. SkeletonPose.SetBoneRotationAbsolute(boneIndex, _bones[i].Rotation); var targetBoneTransform = SkeletonPose.GetBoneTransform(boneIndex); // Apply weight. if (requiresBlending) { BlendBoneTransform(ref originalBoneTransform, ref targetBoneTransform); } // Apply angular velocity limit. if (requiresLimiting) { LimitBoneTransform(ref originalBoneTransform, ref targetBoneTransform, maxRotationAngle); } // Set final bone transform. SkeletonPose.SetBoneTransform(boneIndex, targetBoneTransform); } _originalBoneTransforms.Clear(); } else { // Weight is 1 and angular velocity limit is not active. // --> Just copy the compute rotations. for (int i = 0; i < numberOfBones; i++) { int boneIndex = _boneIndices[i]; SkeletonPose.SetBoneRotationAbsolute(boneIndex, _bones[i].Rotation); } } }
protected static double CalculatePitch(QuaternionF orientation) { return(System.Math.Asin(2.0f * (orientation.W * orientation.Y - orientation.Z * orientation.X))); }
/// <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 = rLow0, Vertex1 = rLow1, Vertex2 = rHigh0, }, false); if (i < numberOfSegments / 4) // At the "northpole" only a triangle is needed. No quad. { mesh.Add(new Triangle { Vertex0 = rLow1, Vertex1 = rHigh1, Vertex2 = rHigh0, }, false); } // Two bottom hemisphere triangles mesh.Add(new Triangle { Vertex0 = -rLow0, Vertex1 = -rHigh0, Vertex2 = -rLow1, }, false); if (i < numberOfSegments / 4) // At the "southpole" only a triangle is needed. No quad. { mesh.Add(new Triangle { Vertex0 = -rLow1, Vertex1 = -rHigh0, Vertex2 = -rHigh1, }, false); } rLow0 = rLow1; rHigh0 = rHigh1; } rLow = rHigh; } mesh.WeldVertices(); return(mesh); }
protected static double CalculateYaw(QuaternionF orientation) { return(System.Math.Atan2(2.0f * (orientation.W * orientation.Z + orientation.X * orientation.Y), 1.0f - 2.0f * (orientation.Y * orientation.Y + orientation.Z * orientation.Z))); }
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 override void Update(GameTime gameTime) { var mousePosition = InputService.MousePosition; var viewport = GraphicsService.GraphicsDevice.Viewport; var cameraNode = GraphicsScreen.CameraNode; // Update picking ray. if (InputService.IsDown(Keys.LeftControl) || InputService.IsDown(Keys.RightControl)) { // Pick using mouse cursor. SampleFramework.IsMouseVisible = true; GraphicsScreen.DrawReticle = false; // Convert the mouse screen position on the near viewing plane into a // world space position. Vector3 rayStart = viewport.Unproject( new Vector3(mousePosition.X, mousePosition.Y, 0), cameraNode.Camera.Projection, (Matrix)cameraNode.View, Matrix.Identity); // Convert the mouse screen position on the far viewing plane into a // world space position. Vector3 rayEnd = viewport.Unproject( new Vector3(mousePosition.X, mousePosition.Y, 1), cameraNode.Camera.Projection, (Matrix)cameraNode.View, Matrix.Identity); // Update ray. The ray should start at the near viewing plane under the // mouse cursor and shoot into viewing direction. Therefore we change the // pose of the ray object such that the ray origin is at rayStart and the // orientation rotates the ray from shooting into +x direction into a ray // shooting in viewing direction (rayEnd - rayStart). ((GeometricObject)_ray.GeometricObject).Pose = new Pose( (Vector3F)rayStart, QuaternionF.CreateRotation(Vector3F.Forward, (Vector3F)(rayEnd - rayStart))); } else { // Pick using reticle. SampleFramework.IsMouseVisible = false; GraphicsScreen.DrawReticle = true; ((GeometricObject)_ray.GeometricObject).Pose = cameraNode.PoseWorld; } // Update collision domain. This computes new contact information. _domain.Update(gameTime.ElapsedGameTime); // Draw objects. Change the color if the ray hits the object. var debugRenderer = GraphicsScreen.DebugRenderer; debugRenderer.Clear(); if (_domain.HaveContact(_ray, _box)) { debugRenderer.DrawObject(_box.GeometricObject, Color.Red, false, false); } else { debugRenderer.DrawObject(_box.GeometricObject, Color.White, false, false); } if (_domain.HaveContact(_ray, _sphere)) { debugRenderer.DrawObject(_sphere.GeometricObject, Color.Red, false, false); } else { debugRenderer.DrawObject(_sphere.GeometricObject, Color.White, false, false); } if (_domain.HaveContact(_ray, _mesh)) { debugRenderer.DrawObject(_mesh.GeometricObject, Color.Red, false, false); } else { debugRenderer.DrawObject(_mesh.GeometricObject, Color.White, false, false); } // For triangle meshes we also know which triangle was hit! // Get the contact information for ray-mesh hits. ContactSet rayMeshContactSet = _domain.ContactSets.GetContacts(_ray, _mesh); // rayMeshContactSet is a collection of Contacts between ray and mesh. // If rayMeshContactSet is null or it contains no Contacts, then we have no contact. if (rayMeshContactSet != null && rayMeshContactSet.Count > 0) { // A contact set contains information for a pair of touching objects A and B. // We know that the two objects are ray and mesh, but we do not know if A or B // is the ray. bool objectAIsRay = rayMeshContactSet.ObjectA == _ray; // Get the contact. Contact contact = rayMeshContactSet[0]; // Get the feature index of the mesh feature that was hit. int featureIndex = objectAIsRay ? contact.FeatureB : contact.FeatureA; // For triangle meshes the feature index is the index of the triangle that was hit. // Get the triangle from the triangle mesh shape. Triangle triangle = ((TriangleMeshShape)_mesh.GeometricObject.Shape).Mesh.GetTriangle(featureIndex); debugRenderer.DrawShape(new TriangleShape(triangle), _mesh.GeometricObject.Pose, _mesh.GeometricObject.Scale, Color.Yellow, false, false); } }
// OnLoad() is called when the GameObject is added to the IGameObjectService. protected override void OnLoad() { var content = _services.GetInstance <ContentManager>(); _skyboxNode = new SkyboxNode(content.Load <TextureCube>("Sky2")) { Color = new Vector3F(SkyExposure), }; // The ambient light. var ambientLight = new AmbientLight { Color = new Vector3F(0.9f, 0.9f, 1f), HdrScale = 0.1f, Intensity = 0.5f, HemisphericAttenuation = 0.8f, }; _ambientLightNode = new LightNode(ambientLight) { Name = "Ambient", }; // The main directional light. var sunlight = new DirectionalLight { Color = new Vector3F(1, 0.9607844f, 0.9078432f), HdrScale = 0.4f, DiffuseIntensity = 1, SpecularIntensity = 1, }; _sunlightNode = new LightNode(sunlight) { Name = "Sunlight", Priority = 10, // This is the most important light. PoseWorld = new Pose(QuaternionF.CreateRotationY(-1.0f) * QuaternionF.CreateRotationX(-1.0f)), // This light uses Cascaded Shadow Mapping. Shadow = new CascadedShadow { PreferredSize = 1024, DepthBiasScale = new Vector4F(0.98f), DepthBiasOffset = new Vector4F(0.0f), FadeOutDistance = 55, MaxDistance = 75, MinLightDistance = 100, ShadowFog = 0.0f, JitterResolution = 3000, SplitDistribution = 0.7f, } }; // Add a lens flare for the key light. var lensFlare = new LensFlare(true) { QuerySize = 0.2f, Size = 0.2f, Name = "Sun Flare" }; var lensFlareTexture = content.Load <Texture2D>("LensFlare/LensFlares"); var circleTexture = new PackedTexture("Circle", lensFlareTexture, new Vector2F(0, 0), new Vector2F(0.25f, 0.5f)); var glowTexture = new PackedTexture("Glow", lensFlareTexture, new Vector2F(0.25f, 0), new Vector2F(0.25f, 0.5f)); var ringTexture = new PackedTexture("Ring", lensFlareTexture, new Vector2F(0.5f, 0), new Vector2F(0.25f, 0.5f)); var haloTexture = new PackedTexture("Halo", lensFlareTexture, new Vector2F(0.75f, 0), new Vector2F(0.25f, 0.5f)); var sunTexture = new PackedTexture("Sun", lensFlareTexture, new Vector2F(0, 0.5f), new Vector2F(0.25f, 0.5f)); var streaksTexture = new PackedTexture("Streaks", lensFlareTexture, new Vector2F(0.25f, 0.5f), new Vector2F(0.25f, 0.5f)); var flareTexture = new PackedTexture("Flare", lensFlareTexture, new Vector2F(0.5f, 0.5f), new Vector2F(0.25f, 0.5f)); lensFlare.Elements.Add(new LensFlareElement(-0.2f, 0.55f, 0.0f, new Color(175, 175, 255, 20), new Vector2F(0.5f, 0.5f), circleTexture)); lensFlare.Elements.Add(new LensFlareElement(0.0f, 0.9f, 0.0f, new Color(255, 255, 255, 255), new Vector2F(0.5f, 0.5f), sunTexture)); lensFlare.Elements.Add(new LensFlareElement(0.0f, 1.8f, 0.0f, new Color(255, 255, 255, 128), new Vector2F(0.5f, 0.5f), streaksTexture)); lensFlare.Elements.Add(new LensFlareElement(0.0f, 2.6f, 0.0f, new Color(255, 255, 200, 64), new Vector2F(0.5f, 0.5f), glowTexture)); lensFlare.Elements.Add(new LensFlareElement(0.5f, 0.12f, 0.0f, new Color(60, 60, 180, 35), new Vector2F(0.5f, 0.5f), circleTexture)); lensFlare.Elements.Add(new LensFlareElement(0.55f, 0.46f, 0.0f, new Color(100, 100, 200, 60), new Vector2F(0.5f, 0.5f), circleTexture)); lensFlare.Elements.Add(new LensFlareElement(0.6f, 0.17f, 0.0f, new Color(120, 120, 220, 40), new Vector2F(0.5f, 0.5f), circleTexture)); lensFlare.Elements.Add(new LensFlareElement(0.85f, 0.2f, 0.0f, new Color(60, 60, 255, 100), new Vector2F(0.5f, 0.5f), ringTexture)); lensFlare.Elements.Add(new LensFlareElement(1.5f, 0.2f, 0.0f, new Color(255, 60, 60, 130), new Vector2F(0.5f, 0.5f), flareTexture)); lensFlare.Elements.Add(new LensFlareElement(0.15f, 0.15f, 0.0f, new Color(255, 60, 60, 90), new Vector2F(0.5f, 0.5f), flareTexture)); lensFlare.Elements.Add(new LensFlareElement(1.3f, 0.6f, 0.0f, new Color(60, 60, 255, 180), new Vector2F(0.5f, 0.5f), haloTexture)); lensFlare.Elements.Add(new LensFlareElement(1.4f, 0.2f, 0.0f, new Color(220, 80, 80, 98), new Vector2F(0.5f, 0.5f), haloTexture)); lensFlare.Elements.Add(new LensFlareElement(1.5f, 0.1f, 0.0f, new Color(220, 80, 80, 85), new Vector2F(0.5f, 0.5f), circleTexture)); lensFlare.Elements.Add(new LensFlareElement(1.6f, 0.5f, 0.0f, new Color(60, 60, 255, 80), new Vector2F(0.5f, 0.5f), haloTexture)); lensFlare.Elements.Add(new LensFlareElement(1.8f, 0.3f, 0.0f, new Color(90, 60, 255, 110), new Vector2F(0.5f, 0.5f), ringTexture)); lensFlare.Elements.Add(new LensFlareElement(1.95f, 0.5f, 0.0f, new Color(60, 60, 255, 120), new Vector2F(0.5f, 0.5f), haloTexture)); lensFlare.Elements.Add(new LensFlareElement(2.0f, 0.15f, 0.0f, new Color(60, 60, 255, 85), new Vector2F(0.5f, 0.5f), circleTexture)); // Add lens flare as a child of the sunlight. var lensFlareNode = new LensFlareNode(lensFlare); _sunlightNode.Children = new SceneNodeCollection(); _sunlightNode.Children.Add(lensFlareNode); // Add scene nodes to scene graph. var scene = _services.GetInstance <IScene>(); scene.Children.Add(_skyboxNode); scene.Children.Add(_ambientLightNode); scene.Children.Add(_sunlightNode); }
private bool _cullingEnabled = true; // True to use frustum culling. False to disable frustum culling. public FrustumCullingSample(Microsoft.Xna.Framework.Game game) : base(game) { GraphicsScreen.ClearBackground = true; GraphicsScreen.BackgroundColor = Color.CornflowerBlue; // The top-down camera. var orthographicProjection = new OrthographicProjection(); orthographicProjection.Set( LevelSize * 1.1f * GraphicsService.GraphicsDevice.Viewport.AspectRatio, LevelSize * 1.1f, 1, 10000f); var topDownCamera = new Camera(orthographicProjection); _topDownCameraNode = new CameraNode(topDownCamera) { View = Matrix44F.CreateLookAt(new Vector3F(0, 1000, 0), new Vector3F(0, 0, 0), -Vector3F.UnitZ), }; // The perspective camera moving through the scene. var perspectiveProjection = new PerspectiveProjection(); perspectiveProjection.SetFieldOfView( MathHelper.ToRadians(45), GraphicsService.GraphicsDevice.Viewport.AspectRatio, 1, 500); var sceneCamera = new Camera(perspectiveProjection); _sceneCameraNode = new CameraNode(sceneCamera); // Initialize collision detection. // We use one collision domain that manages all objects. _domain = new CollisionDomain(new CollisionDetection()) { // We exchange the default broad phase with a DualPartition. The DualPartition // has special support for frustum culling. BroadPhase = new DualPartition <CollisionObject>(), }; // Create a lot of random objects and add them to the collision domain. RandomHelper.Random = new Random(12345); for (int i = 0; i < NumberOfObjects; i++) { // A real scene consists of a lot of complex objects such as characters, vehicles, // buildings, lights, etc. When doing frustum culling we need to test each objects against // the viewing frustum. If it intersects with the viewing frustum, the object is visible // from the camera's point of view. However, in practice we do not test the exact object // against the viewing frustum. Each objects is approximated by a simpler shape. In our // example, we assume that each object is approximated with an oriented bounding box. // (We could also use an other shape, such as a bounding sphere.) // Create a random box. Shape randomShape = new BoxShape(RandomHelper.Random.NextVector3F(1, 10)); // Create a random position. Vector3F randomPosition; randomPosition.X = RandomHelper.Random.NextFloat(-LevelSize / 2, LevelSize / 2); randomPosition.Y = RandomHelper.Random.NextFloat(0, 2); randomPosition.Z = RandomHelper.Random.NextFloat(-LevelSize / 2, LevelSize / 2); // Create a random orientation. QuaternionF randomOrientation = RandomHelper.Random.NextQuaternionF(); // Create object and add it to collision domain. var geometricObject = new GeometricObject(randomShape, new Pose(randomPosition, randomOrientation)); var collisionObject = new CollisionObject(geometricObject) { CollisionGroup = 0, }; _domain.CollisionObjects.Add(collisionObject); } // Per default, the collision domain computes collision between all objects. // In this sample we do not need this information and disable it with a collision // filter. // In a real application, we would use this collision information for rendering, // for example, to find out which lights overlap with which meshes, etc. var filter = new CollisionFilter(); // Disable collision between objects in collision group 0. filter.Set(0, 0, false); _domain.CollisionDetection.CollisionFilter = filter; // Start with the scene camera. GraphicsScreen.CameraNode = _sceneCameraNode; // We will collect a few statistics for debugging. Profiler.SetFormat("NoCull", 1000, "Time in ms to submit DebugRenderer draw jobs without frustum culling."); Profiler.SetFormat("WithCull", 1000, "Time in ms to submit DebugRenderer draw jobs with frustum culling."); }
/// <summary> /// Sets the bone rotation of a bone so that it matches the given rotation in model space. /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <param name="boneIndex">The index of the bone.</param> /// <param name="rotation">The rotation in model space.</param> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose" /> is <see langword="null"/>. /// </exception> public static void SetBoneRotationAbsolute(this SkeletonPose skeletonPose, int boneIndex, QuaternionF rotation) { if (skeletonPose == null) { throw new ArgumentNullException("skeletonPose"); } var skeleton = skeletonPose.Skeleton; var bindPoseRelative = skeleton.GetBindPoseRelative(boneIndex); var parentIndex = skeleton.GetParent(boneIndex); var parentBonePoseAbsolute = (parentIndex >= 0) ? skeletonPose.GetBonePoseAbsolute(parentIndex) : SrtTransform.Identity; // Solving this equation (using only rotations): // rotation = parentBonePoseAbsolute * bindPoseRelative * rotationRelative; // rotationRelative = boneTransform. var rotationRelative = bindPoseRelative.Rotation.Conjugated * parentBonePoseAbsolute.Rotation.Conjugated * rotation; rotationRelative.Normalize(); var boneTransform = skeletonPose.GetBoneTransform(boneIndex); boneTransform.Rotation = rotationRelative; skeletonPose.SetBoneTransform(boneIndex, boneTransform); }
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))); }
protected static double CalculatePitch(QuaternionF orientation) { //Contract.Requires<ArgumentNullException>(orientation != null, "orientation"); return(System.Math.Asin(2.0f * (orientation.W * orientation.Y - orientation.Z * orientation.X))); }
public static QuaternionF Inverse(QuaternionF q) => q.Inverse;
//public static void RotateBoneWorld(this SkeletonPose SkeletonPose, int boneIndex, QuaternionF rotation, Matrix44F world) //{ // QuaternionF worldRotation = QuaternionF.CreateRotation(world.Minor); // RotateBoneAbsolute(SkeletonPose, boneIndex, worldRotation.Conjugated * rotation); //} // TODO: This method should really be called RotateBoneLocalAnimated? ///// <summary> ///// Rotates bone where the rotation is given in the bone space. ///// </summary> ///// <param name="skeletonPose">The skeleton pose.</param> ///// <param name="boneIndex">The index of the bone.</param> ///// <param name="rotation">The rotation in bone space.</param> ///// <exception cref="ArgumentNullException"> ///// <paramref name="skeletonPose" /> is <see langword="null"/>. ///// </exception> //public static void RotateBoneLocal(this SkeletonPose skeletonPose, int boneIndex, QuaternionF rotation) //{ // if (skeletonPose == null) // throw new ArgumentNullException("skeletonPose"); // var boneTransform = skeletonPose.GetBoneTransform(boneIndex); // boneTransform.Rotation = boneTransform.Rotation * rotation; // skeletonPose.SetBoneTransform(boneIndex, boneTransform); //} /// <summary> /// Rotates a bone where the rotation is given in model space. /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <param name="boneIndex">The index of the bone.</param> /// <param name="rotation">The rotation in model space.</param> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose" /> is <see langword="null"/>. /// </exception> public static void RotateBoneAbsolute(this SkeletonPose skeletonPose, int boneIndex, QuaternionF rotation) { if (skeletonPose == null) throw new ArgumentNullException("skeletonPose"); var skeleton = skeletonPose.Skeleton; var boneTransform = skeletonPose.GetBoneTransform(boneIndex); var bindPoseRelative = skeleton.GetBindPoseRelative(boneIndex); var parentIndex = skeleton.GetParent(boneIndex); var parentBonePoseAbsolute = skeletonPose.GetBonePoseAbsolute(parentIndex); // Solving these equations (using only the rotations): // rotation * bonePoseAbsolute = bonePoseAbsoluteNew // bonePoseAbsolute = parentBonePoseAbsolute * bindPoseRelative * boneTransform. // ... // Rotation relative to bone bind pose space (using similarity transformation). var rotationRelative = bindPoseRelative.Rotation.Conjugated * parentBonePoseAbsolute.Rotation.Conjugated * rotation * parentBonePoseAbsolute.Rotation * bindPoseRelative.Rotation; // The final rotation is: First rotate into bone bind pose space, then apply rotation. boneTransform.Rotation = rotationRelative * boneTransform.Rotation; // So many multiplications, numerical errors adds up quickly in iterative IK algorithms... boneTransform.Rotation.Normalize(); skeletonPose.SetBoneTransform(boneIndex, boneTransform); }
public static QuaternionF Normalized(QuaternionF q) => q.Normalized;
private void CreateRoad() { //RandomHelper.Random = new Random(1234567); // Set isClosed to true join the start and the end of the road. bool isClosed = false; // Create a new TerrainRoadLayer which paints a road onto the terrain. // The road itself is defined by a mesh which is set later. _roadLayer = new TerrainRoadLayer(GraphicsService) { DiffuseColor = new Vector3F(0.5f), SpecularColor = new Vector3F(1), DiffuseTexture = ContentManager.Load <Texture2D>("Terrain/Road-Asphalt-Diffuse"), NormalTexture = ContentManager.Load <Texture2D>("Terrain/Road-Asphalt-Normal"), SpecularTexture = ContentManager.Load <Texture2D>("Terrain/Road-Asphalt-Specular"), HeightTexture = ContentManager.Load <Texture2D>("Terrain/Road-Asphalt-Height"), // The size of the tileable detail textures in world space units. TileSize = 5, // The border blend range controls how the border of the road fades out. // We fade out 5% of the texture on each side of the road. BorderBlendRange = new Vector4F(0.05f, 0.05f, 0.05f, 0.05f), }; // Create 3D spline path with some semi-random control points. _roadPath = new Path3F { PreLoop = isClosed ? CurveLoopType.Cycle : CurveLoopType.Linear, PostLoop = isClosed ? CurveLoopType.Cycle : CurveLoopType.Linear, SmoothEnds = isClosed, }; // The position of the next path key. Vector3F position = new Vector3F( RandomHelper.Random.NextFloat(-20, 20), 0, RandomHelper.Random.NextFloat(-20, 20)); // The direction to the next path key. Vector3F direction = QuaternionF.CreateRotationY(RandomHelper.Random.NextFloat(0, 10)).Rotate(Vector3F.Forward); // Add path keys. for (int j = 0; j < 10; j++) { // Instead of a normal PathKey3F, we use a TerrainRoadPathKey which allows to control // the road with and the side falloff. var key = new TerrainRoadPathKey { Interpolation = SplineInterpolation.CatmullRom, Parameter = j, Point = position, // The width of the road at the path key. Width = RandomHelper.Random.NextFloat(6, 10), // The side falloff (which is used in ClampTerrainToRoad to blend the height values with // the road). SideFalloff = RandomHelper.Random.NextFloat(20, 40), }; _roadPath.Add(key); // Get next random position and direction. position += direction * RandomHelper.Random.NextFloat(20, 40); position.Y += RandomHelper.Random.NextFloat(-2, 2); direction = QuaternionF.CreateRotationY(RandomHelper.Random.NextFloat(-1, 1)) .Rotate(direction); } if (isClosed) { // To create a closed path the first and the last key should be identical. _roadPath[_roadPath.Count - 1].Point = _roadPath[0].Point; ((TerrainRoadPathKey)_roadPath[_roadPath.Count - 1]).Width = ((TerrainRoadPathKey)_roadPath[0]).Width; ((TerrainRoadPathKey)_roadPath[_roadPath.Count - 1]).SideFalloff = ((TerrainRoadPathKey)_roadPath[0]).SideFalloff; // Since the path is closed we do not have to fade out the start and the end of the road. _roadLayer.BorderBlendRange *= new Vector4F(1, 0, 1, 0); } // Convert the path to a mesh. Submesh roadSubmesh; Aabb roadAabb; float roadLength; TerrainRoadLayer.CreateMesh( GraphicsService.GraphicsDevice, _roadPath, 4, 10, 0.1f, out roadSubmesh, out roadAabb, out roadLength); // Set the mesh in the road layer. _roadLayer.SetMesh(roadSubmesh, roadAabb, roadLength, true); if (isClosed) { // When the path is closed, the end texture and the start texture coordinates should // match. This is the case if (roadLength / tileSize) is an integer. var numberOfTiles = (int)(roadLength / _roadLayer.TileSize); _roadLayer.TileSize = roadLength / numberOfTiles; } // The road layer is added to the layers of a tile. We have to add the road to each terrain // tile which it overlaps. foreach (var tile in _terrainObject.TerrainNode.Terrain.Tiles) { if (GeometryHelper.HaveContact(tile.Aabb, _roadLayer.Aabb.Value)) { tile.Layers.Add(_roadLayer); } } }
protected override void OnUpdate(TimeSpan deltaTime) { MousePicking(); if (_inputService.IsDown(Keys.W)) { _position -= (deltaTime.Milliseconds / 10.0f) * new Vector3F(GetForwardVector().X, 0.0f, GetForwardVector().Z); } if (_inputService.IsDown(Keys.A)) { _position -= (deltaTime.Milliseconds / 10.0f) * GetRightVector(); } if (_inputService.IsDown(Keys.S)) { _position += (deltaTime.Milliseconds / 10.0f) * new Vector3F(GetForwardVector().X, 0.0f, GetForwardVector().Z); } if (_inputService.IsDown(Keys.D)) { _position += (deltaTime.Milliseconds / 10.0f) * GetRightVector(); } if (_inputService.IsDown(Keys.E)) { _pitch -= Microsoft.Xna.Framework.MathHelper.ToRadians(deltaTime.Milliseconds / 20.0f); } if (_inputService.IsDown(Keys.Q)) { _pitch += Microsoft.Xna.Framework.MathHelper.ToRadians(deltaTime.Milliseconds / 20.0f); } _position -= new Vector3F(0.0f, (_inputService.MouseWheelDelta / 20.0f) * GetUpVector().Y, 0.0f); _orientation = QuaternionF.CreateRotationY(_pitch) * QuaternionF.CreateRotationX(_yaw); _position.X = Microsoft.Xna.Framework.MathHelper.Clamp(_position.X, -250.0f, 250.0f); _position.Y = Microsoft.Xna.Framework.MathHelper.Clamp(_position.Y, 60.0f, 270.0f); _position.Z = Microsoft.Xna.Framework.MathHelper.Clamp(_position.Z, -250.0f, 250.0f); _cameraNode.PoseWorld = new Pose(_position, _orientation); View = _cameraNode.View.ToXna(); Projection = _cameraNode.Camera.Projection.ToXna(); base.OnUpdate(deltaTime); }
public void GetEventOrientation_ValidParameters_ExpectedBridgeCalls() { // Setup var myoHandle = new IntPtr(123); var eventHandle = new IntPtr(789); var expectedResult = new QuaternionF(10, 20, 30, 40); var bridge = new Mock <IMyoDeviceBridge>(MockBehavior.Strict); bridge .Setup(x => x.EventGetOrientation32(eventHandle, OrientationIndex.W)) .Returns(expectedResult.W); bridge .Setup(x => x.EventGetOrientation32(eventHandle, OrientationIndex.X)) .Returns(expectedResult.X); bridge .Setup(x => x.EventGetOrientation32(eventHandle, OrientationIndex.Y)) .Returns(expectedResult.Y); bridge .Setup(x => x.EventGetOrientation32(eventHandle, OrientationIndex.Z)) .Returns(expectedResult.Z); bridge .Setup(x => x.EventGetOrientation64(eventHandle, OrientationIndex.W)) .Returns(expectedResult.W); bridge .Setup(x => x.EventGetOrientation64(eventHandle, OrientationIndex.X)) .Returns(expectedResult.X); bridge .Setup(x => x.EventGetOrientation64(eventHandle, OrientationIndex.Y)) .Returns(expectedResult.Y); bridge .Setup(x => x.EventGetOrientation64(eventHandle, OrientationIndex.Z)) .Returns(expectedResult.Z); var myoErrorHandlerDriver = new Mock <IMyoErrorHandlerDriver>(MockBehavior.Strict); var driver = MyoDeviceDriver.Create( myoHandle, bridge.Object, myoErrorHandlerDriver.Object); // Execute var result = driver.GetEventOrientation(eventHandle); // Assert Assert.Equal(expectedResult.W, result.W); Assert.Equal(expectedResult.X, result.X); Assert.Equal(expectedResult.Y, result.Y); Assert.Equal(expectedResult.Z, result.Z); bridge.Verify(x => x.EventGetOrientation32(eventHandle, OrientationIndex.W), PlatformInvocation.Running32Bit ? Times.Once() : Times.Never()); bridge.Verify(x => x.EventGetOrientation32(eventHandle, OrientationIndex.X), PlatformInvocation.Running32Bit ? Times.Once() : Times.Never()); bridge.Verify(x => x.EventGetOrientation32(eventHandle, OrientationIndex.Y), PlatformInvocation.Running32Bit ? Times.Once() : Times.Never()); bridge.Verify(x => x.EventGetOrientation32(eventHandle, OrientationIndex.Z), PlatformInvocation.Running32Bit ? Times.Once() : Times.Never()); bridge.Verify(x => x.EventGetOrientation64(eventHandle, OrientationIndex.W), PlatformInvocation.Running32Bit ? Times.Never() : Times.Once()); bridge.Verify(x => x.EventGetOrientation64(eventHandle, OrientationIndex.X), PlatformInvocation.Running32Bit ? Times.Never() : Times.Once()); bridge.Verify(x => x.EventGetOrientation64(eventHandle, OrientationIndex.Y), PlatformInvocation.Running32Bit ? Times.Never() : Times.Once()); bridge.Verify(x => x.EventGetOrientation64(eventHandle, OrientationIndex.Z), PlatformInvocation.Running32Bit ? Times.Never() : Times.Once()); }
// Initializes the bone rotations using the quaternions of the specified array. private void ArrayToSkeletonPose(float[] values) { var numberOfBones = SkeletonPose.Skeleton.NumberOfBones; for (int i = 0; i < numberOfBones; i++) { QuaternionF quaternion = new QuaternionF( values[i * 4 + 0], values[i * 4 + 1], values[i * 4 + 2], values[i * 4 + 3]); // The quaternions were filtered using component-wise linear interpolation. This // is only an approximation which denormalizes the quaternions. // --> Renormalize the quaternions. quaternion.TryNormalize(); // Exchange the rotation in the bone transform. var boneTransform = SkeletonPose.GetBoneTransform(i); boneTransform.Rotation = quaternion; SkeletonPose.SetBoneTransform(i, boneTransform); } }
public ShapesSample(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); // ----- Add a sphere. Shape sphere = new SphereShape(0.5f); Simulation.RigidBodies.Add(new RigidBody(sphere)); // ----- Add a box. BoxShape box = new BoxShape(0.5f, 0.9f, 0.7f); Simulation.RigidBodies.Add(new RigidBody(box)); // ----- Add a capsule. CapsuleShape capsule = new CapsuleShape(0.4f, 1.2f); Simulation.RigidBodies.Add(new RigidBody(capsule)); // ----- Add a cone. ConeShape cone = new ConeShape(0.5f, 1f); Simulation.RigidBodies.Add(new RigidBody(cone)); // ----- Add a cylinder. CylinderShape cylinder = new CylinderShape(0.3f, 1f); Simulation.RigidBodies.Add(new RigidBody(cylinder)); // ----- Add a convex hull of random points. ConvexHullOfPoints convexHullOfPoints = new ConvexHullOfPoints(); for (int i = 0; i < 20; i++) { convexHullOfPoints.Points.Add(RandomHelper.Random.NextVector3F(-0.5f, 0.5f)); } Simulation.RigidBodies.Add(new RigidBody(convexHullOfPoints)); // ----- Add a convex polyhedron. // (A ConvexPolyhedron is similar to the ConvexHullOfPoints. The difference is that // the points in a ConvexHullOfPoints can be changed at runtime. A ConvexPolyhedron // cannot be changed at runtime, but it is faster.) List <Vector3F> points = new List <Vector3F>(); for (int i = 0; i < 20; i++) { points.Add(RandomHelper.Random.NextVector3F(-0.7f, 0.7f)); } ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(points); Simulation.RigidBodies.Add(new RigidBody(convexPolyhedron)); // ----- Add a composite shape (a table that consists of 5 boxes). CompositeShape composite = new CompositeShape(); composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(-0.75f, 0.4f, -0.5f)))); composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(0.75f, 0.4f, -0.5f)))); composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(-0.75f, 0.4f, 0.5f)))); composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(0.75f, 0.4f, 0.5f)))); composite.Children.Add(new GeometricObject(new BoxShape(1.8f, 0.1f, 1.1f), new Pose(new Vector3F(0, 0.8f, 0)))); Simulation.RigidBodies.Add(new RigidBody(composite)); // ----- Add a convex hull of multiple shapes. ConvexHullOfShapes convexHullOfShapes = new ConvexHullOfShapes(); convexHullOfShapes.Children.Add(new GeometricObject(new CylinderShape(0.2f, 0.8f), new Pose(new Vector3F(-0.4f, 0, 0)))); convexHullOfShapes.Children.Add(new GeometricObject(new CylinderShape(0.2f, 0.8f), new Pose(new Vector3F(+0.4f, 0, 0)))); Simulation.RigidBodies.Add(new RigidBody(convexHullOfShapes)); // ----- Add the Minkowski sum of two shapes. // (The Minkowski sum is a mathematical operation that combines two shapes. // Here a circle is combined with a sphere. The result is a wheel.) MinkowskiSumShape minkowskiSum = new MinkowskiSumShape(); minkowskiSum.ObjectA = new GeometricObject(new SphereShape(0.2f), Pose.Identity); minkowskiSum.ObjectB = new GeometricObject(new CircleShape(0.5f), Pose.Identity); Simulation.RigidBodies.Add(new RigidBody(minkowskiSum)); // Create another Minkowski sum. (Here a small sphere is added to a box to create a // box with rounded corners.) minkowskiSum = new MinkowskiSumShape(); minkowskiSum.ObjectA = new GeometricObject(new SphereShape(0.1f), Pose.Identity); minkowskiSum.ObjectB = new GeometricObject(new BoxShape(0.2f, 0.5f, 0.8f), Pose.Identity); Simulation.RigidBodies.Add(new RigidBody(minkowskiSum)); // ----- Add a triangle mesh. // A triangle mesh could be loaded from a file or built from an XNA model. // Here we first create a composite shape and convert the shape into a triangle // mesh. (Any Shape in DigitalRune.Geometry can be converted to a triangle mesh.) CompositeShape dumbbell = new CompositeShape(); dumbbell.Children.Add(new GeometricObject(new SphereShape(0.4f), new Pose(new Vector3F(0.6f, 0.0f, 0.0f)))); dumbbell.Children.Add(new GeometricObject(new SphereShape(0.4f), new Pose(new Vector3F(-0.6f, 0.0f, 0.0f)))); dumbbell.Children.Add(new GeometricObject(new CylinderShape(0.1f, 0.6f), new Pose(Matrix33F.CreateRotationZ(ConstantsF.PiOver2)))); TriangleMeshShape triangleMeshShape = new TriangleMeshShape(dumbbell.GetMesh(0.01f, 2)); // Optional: We can enable "contact welding". When this flag is enabled, the triangle shape // precomputes additional internal information for the mesh. The collision detection will // be able to compute better contacts (e.g. better normal vectors at triangle edges). // Pro: Collision detection can compute better contact information. // Con: Contact welding information needs a bit of memory. And the collision detection is // a bit slower. triangleMeshShape.EnableContactWelding = true; // Optional: Assign a spatial partitioning scheme to the triangle mesh. (A spatial partition // adds an additional memory overhead, but it improves collision detection speed tremendously!) triangleMeshShape.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, }; Simulation.RigidBodies.Add(new RigidBody(triangleMeshShape)); // ----- Set random positions/orientations. // (Start with the second object. The first object is the ground plane which should // not be changed.) for (int i = 1; i < Simulation.RigidBodies.Count; i++) { RigidBody body = Simulation.RigidBodies[i]; Vector3F position = RandomHelper.Random.NextVector3F(-3, 3); position.Y = 3; // Position the objects 3m above ground. QuaternionF orientation = RandomHelper.Random.NextQuaternionF(); body.Pose = new Pose(position, orientation); } }
public void LengthSquared() { QuaternionF q = new QuaternionF(1.0f, 2.0f, 3.0f, 4.0f); float lengthSquared = 1 + 4 + 9 + 16; Assert.AreEqual(lengthSquared, q.Norm); }
public static QuaternionF Conjugated(QuaternionF q) => q.Conjugated;
public void Ln4() { float θ = 0.0f; Vector3F v = new Vector3F(1.0f, 2.0f, 3.0f); v.Normalize(); QuaternionF q = new QuaternionF((float)Math.Cos(θ), (float)Math.Sin(θ) * v); q.Ln(); Assert.IsTrue(Numeric.AreEqual(0.0f, q.W)); Assert.IsTrue(Vector3F.AreNumericallyEqual(θ * v, q.V)); }
private static void Append(StringBuilder text, QuaternionF q) { text.Append(string.Format(CultureInfo.InvariantCulture, "new QuaternionF({0}f, {1}f, {2}f, {3}f)", q.W, q.X, q.Y, q.Z)); }
public static float Dot(this QuaternionF a, QuaternionF b) { return(a.W * b.W + a.X * b.X + a.Y * b.Y + a.Z * b.Z); }
private static Vector4 ToVector(QuaternionF quaternion) { return new Vector4(quaternion.X, quaternion.Y, quaternion.Z, quaternion.W); }
public static float NormSquared(QuaternionF q) => q.NormSquared;