/// <summary> /// Sets up the joint transforms by automatically creating perpendicular vectors to complete the bases. /// </summary> /// <param name="worldTwistAxisA">Twist axis in world space to attach to entity A.</param> /// <param name="worldTwistAxisB">Twist axis in world space to attach to entity B.</param> public void SetupJointTransforms(Vector3 worldTwistAxisA, Vector3 worldTwistAxisB) { worldTwistAxisA.Normalize(); worldTwistAxisB.Normalize(); Vector3 worldXAxis; Vector3.Cross(ref worldTwistAxisA, ref Toolbox.UpVector, out worldXAxis); float length = worldXAxis.LengthSquared(); if (length < Toolbox.Epsilon) { Vector3.Cross(ref worldTwistAxisA, ref Toolbox.RightVector, out worldXAxis); } worldXAxis.Normalize(); //Complete A's basis. Vector3 worldYAxis; Vector3.Cross(ref worldTwistAxisA, ref worldXAxis, out worldYAxis); BasisA.rotationMatrix = connectionA.orientationMatrix; BasisA.SetWorldAxes(worldTwistAxisA, worldXAxis, worldYAxis); //Rotate the axis to B since it could be arbitrarily rotated. Quaternion rotation; Quaternion.GetQuaternionBetweenNormalizedVectors(ref worldTwistAxisA, ref worldTwistAxisB, out rotation); Quaternion.Transform(ref worldXAxis, ref rotation, out worldXAxis); BasisB.rotationMatrix = connectionB.orientationMatrix; BasisB.SetWorldAxes(worldTwistAxisB, worldXAxis); }
/// <summary> /// Do any necessary computations to prepare the constraint for this frame. /// </summary> /// <param name="dt">Simulation step length.</param> public override void Update(float dt) { BasisA.rotationMatrix = connectionA.orientationMatrix; BasisB.rotationMatrix = connectionB.orientationMatrix; BasisA.ComputeWorldSpaceAxes(); BasisB.ComputeWorldSpaceAxes(); if (Settings.mode == MotorMode.Servomechanism) { Quaternion rotation; Quaternion.GetQuaternionBetweenNormalizedVectors(ref BasisB.primaryAxis, ref BasisA.primaryAxis, out rotation); //Transform b's 'Y' axis so that it is perpendicular with a's 'X' axis for measurement. Vector3 twistMeasureAxis; Quaternion.Transform(ref BasisB.xAxis, ref rotation, out twistMeasureAxis); //By dotting the measurement vector with a 2d plane's axes, we can get a local X and Y value. float y, x; Vector3.Dot(ref twistMeasureAxis, ref BasisA.yAxis, out y); Vector3.Dot(ref twistMeasureAxis, ref BasisA.xAxis, out x); float angle = (float)Math.Atan2(y, x); //Compute goal velocity. Error = GetDistanceFromGoal(angle); float absErrorOverDt = Math.Abs(Error / dt); float errorReduction; Settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out usedSoftness); biasVelocity = Math.Sign(Error) * MathHelper.Min(Settings.servo.baseCorrectiveSpeed, absErrorOverDt) + Error * errorReduction; biasVelocity = MathHelper.Clamp(biasVelocity, -Settings.servo.maxCorrectiveVelocity, Settings.servo.maxCorrectiveVelocity); } else { biasVelocity = Settings.velocityMotor.goalVelocity; usedSoftness = Settings.velocityMotor.softness / dt; Error = 0; } //The nice thing about this approach is that the jacobian entry doesn't flip. //Instead, the error can be negative due to the use of Atan2. //This is important for limits which have a unique high and low value. //Compute the jacobian. Vector3.Add(ref BasisA.primaryAxis, ref BasisB.primaryAxis, out jacobianB); if (jacobianB.LengthSquared() < Toolbox.Epsilon) { //A nasty singularity can show up if the axes are aligned perfectly. //In a 'real' situation, this is impossible, so just ignore it. isActiveInSolver = false; return; } jacobianB.Normalize(); jacobianA.X = -jacobianB.X; jacobianA.Y = -jacobianB.Y; jacobianA.Z = -jacobianB.Z; //Update the maximum force ComputeMaxForces(Settings.maximumForce, dt); //****** EFFECTIVE MASS MATRIX ******// //Connection A's contribution to the mass matrix float entryA; Vector3 transformedAxis; if (connectionA.isDynamic) { Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA); } else { entryA = 0; } //Connection B's contribution to the mass matrix float entryB; if (connectionB.isDynamic) { Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB); } else { entryB = 0; } //Compute the inverse mass matrix velocityToImpulse = 1 / (usedSoftness + entryA + entryB); }