/// <summary> /// Modifies a contribution using a transform, position, and weight. /// </summary> /// <param name="transform">Transform to use to modify the contribution.</param> /// <param name="center">Center to use to modify the contribution.</param> /// <param name="baseContribution">Original unmodified contribution.</param> /// <param name="weight">Weight of the contribution.</param> /// <param name="contribution">Transformed contribution.</param> public static void TransformContribution(ref RigidTransform transform, ref Vector3 center, ref Matrix3x3 baseContribution, float weight, out Matrix3x3 contribution) { Matrix3x3 rotation; Matrix3x3.CreateFromQuaternion(ref transform.Orientation, out rotation); Matrix3x3 temp; //Do angular transformed contribution first... Matrix3x3.MultiplyTransposed(ref rotation, ref baseContribution, out temp); Matrix3x3.Multiply(ref temp, ref rotation, out temp); contribution = temp; //Now add in the offset from the origin. Vector3 offset; Vector3.Subtract(ref transform.Position, ref center, out offset); Matrix3x3 innerProduct; Matrix3x3.CreateScale(offset.LengthSquared(), out innerProduct); Matrix3x3 outerProduct; Matrix3x3.CreateOuterProduct(ref offset, ref offset, out outerProduct); Matrix3x3.Subtract(ref innerProduct, ref outerProduct, out temp); Matrix3x3.Add(ref contribution, ref temp, out contribution); Matrix3x3.Multiply(ref contribution, weight, out contribution); }
/// <summary> /// Computes a convex shape description for a TransformableShape. /// </summary> ///<param name="vA">First local vertex in the triangle.</param> ///<param name="vB">Second local vertex in the triangle.</param> ///<param name="vC">Third local vertex in the triangle.</param> ///<param name="collisionMargin">Collision margin of the shape.</param> /// <returns>Description required to define a convex shape.</returns> public static ConvexShapeDescription ComputeDescription(Vector3 vA, Vector3 vB, Vector3 vC, float collisionMargin) { ConvexShapeDescription description; // A triangle by itself technically has no volume, but shapes try to include the collision margin in the volume when feasible (e.g. BoxShape). //Plus, it's convenient to have a nonzero volume for buoyancy. var doubleArea = Vector3.Cross(vB - vA, vC - vA).Length(); description.EntityShapeVolume.Volume = doubleArea * collisionMargin; //Compute the inertia tensor. var v = new Matrix3x3( vA.X, vA.Y, vA.Z, vB.X, vB.Y, vB.Z, vC.X, vC.Y, vC.Z); var s = new Matrix3x3( 2, 1, 1, 1, 2, 1, 1, 1, 2); Matrix3x3.MultiplyTransposed(ref v, ref s, out description.EntityShapeVolume.VolumeDistribution); Matrix3x3.Multiply(ref description.EntityShapeVolume.VolumeDistribution, ref v, out description.EntityShapeVolume.VolumeDistribution); var scaling = doubleArea / 24f; Matrix3x3.Multiply(ref description.EntityShapeVolume.VolumeDistribution, -scaling, out description.EntityShapeVolume.VolumeDistribution); //The square-of-sum term is ignored since the parameters should already be localized (and so would sum to zero). var sums = scaling * (vA.LengthSquared() + vB.LengthSquared() + vC.LengthSquared()); description.EntityShapeVolume.VolumeDistribution.M11 += sums; description.EntityShapeVolume.VolumeDistribution.M22 += sums; description.EntityShapeVolume.VolumeDistribution.M33 += sums; description.MinimumRadius = collisionMargin; description.MaximumRadius = collisionMargin + Math.Max(vA.Length(), Math.Max(vB.Length(), vC.Length())); description.CollisionMargin = collisionMargin; return(description); }
void IForceUpdateable.UpdateForForces(float dt) { //Apply gravity. if (hasPersonalGravity) { Vector3 gravityDt; Vector3.Multiply(ref personalGravity, dt, out gravityDt); Vector3.Add(ref gravityDt, ref linearVelocity, out linearVelocity); } else { Vector3.Add(ref forceUpdater.gravityDt, ref linearVelocity, out linearVelocity); } //Boost damping at very low velocities. This is a strong stabilizer; removes a ton of energy from the system. if (activityInformation.DeactivationManager.useStabilization && activityInformation.allowStabilization && (activityInformation.isSlowing || activityInformation.velocityTimeBelowLimit > activityInformation.DeactivationManager.lowVelocityTimeMinimum)) { float energy = linearVelocity.LengthSquared() + angularVelocity.LengthSquared(); if (energy < activityInformation.DeactivationManager.velocityLowerLimitSquared) { float boost = 1 - (float)(Math.Sqrt(energy) / (2f * activityInformation.DeactivationManager.velocityLowerLimit)); ModifyAngularDamping(boost); ModifyLinearDamping(boost); } } //Damping float linear = LinearDamping + linearDampingBoost; if (linear > 0) { Vector3.Multiply(ref linearVelocity, (float)Math.Pow(MathHelper.Clamp(1 - linear, 0, 1), dt), out linearVelocity); } //When applying angular damping, the momentum or velocity is damped depending on the conservation setting. float angular = AngularDamping + angularDampingBoost; if (angular > 0) { #if CONSERVE Vector3.Multiply(ref angularMomentum, (float)Math.Pow(MathHelper.Clamp(1 - angular, 0, 1), dt), out angularMomentum); #else Vector3.Multiply(ref angularVelocity, (float)Math.Pow(MathHelper.Clamp(1 - angular, 0, 1), dt), out angularVelocity); #endif } linearDampingBoost = 0; angularDampingBoost = 0; //Update world inertia tensors. Matrix3x3 multiplied; Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensorInverse, out multiplied); Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensorInverse); Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensor, out multiplied); Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensor); #if CONSERVE //Update angular velocity. //Note that this doesn't play nice with singular inertia tensors. //Locked tensors result in zero angular velocity. Matrix3x3.Transform(ref angularMomentum, ref inertiaTensorInverse, out angularVelocity); MathChecker.Validate(angularMomentum); #endif MathChecker.Validate(linearVelocity); MathChecker.Validate(angularVelocity); }