protected override TriangleCollidable GetOpposingCollidable(int index)
{
//Construct a TriangleCollidable from the static mesh.
var toReturn = PhysicsResources.GetTriangleCollidable();
var shape = toReturn.Shape;
mesh.Shape.TriangleMesh.Data.GetTriangle(index, out shape.vA, out shape.vB, out shape.vC);
Matrix3x3.Transform(ref shape.vA, ref mesh.worldTransform.LinearTransform, out shape.vA);
Matrix3x3.Transform(ref shape.vB, ref mesh.worldTransform.LinearTransform, out shape.vB);
Matrix3x3.Transform(ref shape.vC, ref mesh.worldTransform.LinearTransform, out shape.vC);
Vector3 center;
Vector3.Add(ref shape.vA, ref shape.vB, out center);
Vector3.Add(ref center, ref shape.vC, out center);
Vector3.Multiply(ref center, 1 / 3f, out center);
Vector3.Subtract(ref shape.vA, ref center, out shape.vA);
Vector3.Subtract(ref shape.vB, ref center, out shape.vB);
Vector3.Subtract(ref shape.vC, ref center, out shape.vC);
Vector3.Add(ref center, ref mesh.worldTransform.Translation, out center);
//The bounding box doesn't update by itself.
toReturn.worldTransform.Position = center;
toReturn.worldTransform.Orientation = Quaternion.Identity;
toReturn.UpdateBoundingBoxInternal(0);
shape.sidedness = mesh.Sidedness;
shape.collisionMargin = mobileMesh.Shape.MeshCollisionMargin;
return(toReturn);
}
/// <summary>
/// Rotates the view direction up or down relative to the locked up vector.
/// </summary>
/// <param name="radians">Amount to rotate.</param>
public void Pitch(float radians)
{
//Do not allow the new view direction to violate the maximum pitch.
float dot;
Vector3.Dot(ref viewDirection, ref lockedUp, out dot);
//While this could be rephrased in terms of dot products alone, converting to actual angles can be more intuitive.
//Consider +Pi/2 to be up, and -Pi/2 to be down.
float currentPitch = (float)Math.Acos(MathHelper.Clamp(-dot, -1, 1)) - MathHelper.PiOver2;
//Compute our new pitch by clamping the current + change.
float newPitch = MathHelper.Clamp(currentPitch + radians, -maximumPitch, maximumPitch);
float allowedChange = newPitch - currentPitch;
//Compute and apply the rotation.
Vector3 pitchAxis;
Vector3.Cross(ref viewDirection, ref lockedUp, out pitchAxis);
//This is guaranteed safe by all interaction points stopping viewDirection from being aligned with lockedUp.
pitchAxis.Normalize();
Matrix3x3 rotation;
Matrix3x3.CreateFromAxisAngle(ref pitchAxis, allowedChange, out rotation);
Matrix3x3.Transform(ref viewDirection, ref rotation, out viewDirection);
//Avoid drift by renormalizing.
viewDirection.Normalize();
}
///<summary>
/// Gets the extreme point of the shape in local space in a given direction.
///</summary>
///<param name="direction">Direction to find the extreme point in.</param>
///<param name="extremePoint">Extreme point on the shape.</param>
public override void GetLocalExtremePointWithoutMargin(ref Vector3 direction, out Vector3 extremePoint)
{
Vector3 d;
Matrix3x3.TransformTranspose(ref direction, ref transform, out d);
shape.GetLocalExtremePoint(d, out extremePoint);
Matrix3x3.Transform(ref extremePoint, ref transform, out extremePoint);
}
/// <summary>
/// Performs any per-frame computation needed by the constraint.
/// </summary>
/// <param name="dt">Time step duration.</param>
public override void Update(float dt)
{
//Collect references, pick the mode, and configure the coefficients to be used by the solver.
if (supportData.SupportObject != null)
{
//Get an easy reference to the support.
var support = supportData.SupportObject as EntityCollidable;
if (support != null)
{
supportEntity = support.Entity;
}
else
{
supportEntity = null;
}
}
else
{
supportEntity = null;
}
maximumForce = maximumGlueForce * dt;
//If we don't allow the character to get out of the ground, it could apply some significant forces to a dynamic support object.
//Let the character escape penetration in a controlled manner. This mirrors the regular penetration recovery speed.
//Since the vertical motion constraint works in the opposite direction of the contact penetration constraint,
//this actually eliminates the 'bounce' that can occur with non-character objects in deep penetration.
permittedVelocity = Math.Min(Math.Max(supportData.Depth * CollisionResponseSettings.PenetrationRecoveryStiffness / dt, 0), CollisionResponseSettings.MaximumPenetrationRecoverySpeed);
//Compute the jacobians and effective mass matrix. This constraint works along a single degree of freedom, so the mass matrix boils down to a scalar.
linearJacobianA = supportData.Normal;
Vector3.Negate(ref linearJacobianA, out linearJacobianB);
float inverseEffectiveMass = characterBody.InverseMass;
if (supportEntity != null)
{
Vector3 offsetB = supportData.Position - supportEntity.Position;
Vector3.Cross(ref offsetB, ref linearJacobianB, out angularJacobianB);
if (supportEntity.IsDynamic)
{
//Only dynamic entities can actually contribute anything to the effective mass.
//Kinematic entities have infinite mass and inertia, so this would all zero out.
Matrix3x3 inertiaInverse = supportEntity.InertiaTensorInverse;
Vector3 angularComponentB;
Matrix3x3.Transform(ref angularJacobianB, ref inertiaInverse, out angularComponentB);
float effectiveMassContribution;
Vector3.Dot(ref angularComponentB, ref angularJacobianB, out effectiveMassContribution);
inverseEffectiveMass += supportForceFactor * (effectiveMassContribution + supportEntity.InverseMass);
}
}
effectiveMass = 1f / (inverseEffectiveMass);
//So much nicer and shorter than the horizontal constraint!
}
/// <summary>
/// Calculates and applies corrective impulses.
/// Called automatically by space.
/// </summary>
public override float SolveIteration()
{
#if !WINDOWS
Vector3 lambda = new Vector3();
#else
Vector3 lambda;
#endif
//Velocity along the length.
Vector3 cross;
Vector3 aVel, bVel;
Vector3.Cross(ref connectionA.angularVelocity, ref worldOffsetA, out cross);
Vector3.Add(ref connectionA.linearVelocity, ref cross, out aVel);
Vector3.Cross(ref connectionB.angularVelocity, ref worldOffsetB, out cross);
Vector3.Add(ref connectionB.linearVelocity, ref cross, out bVel);
lambda.X = aVel.X - bVel.X + biasVelocity.X - softness * accumulatedImpulse.X;
lambda.Y = aVel.Y - bVel.Y + biasVelocity.Y - softness * accumulatedImpulse.Y;
lambda.Z = aVel.Z - bVel.Z + biasVelocity.Z - softness * accumulatedImpulse.Z;
//Turn the velocity into an impulse.
Matrix3x3.Transform(ref lambda, ref massMatrix, out lambda);
//Accumulate the impulse
Vector3.Add(ref accumulatedImpulse, ref lambda, out accumulatedImpulse);
//Apply the impulse
//Constraint.applyImpulse(myConnectionA, myConnectionB, ref rA, ref rB, ref impulse);
#if !WINDOWS
Vector3 linear = new Vector3();
#else
Vector3 linear;
#endif
if (connectionA.isDynamic)
{
linear.X = -lambda.X;
linear.Y = -lambda.Y;
linear.Z = -lambda.Z;
connectionA.ApplyLinearImpulse(ref linear);
Vector3 taImpulse;
Vector3.Cross(ref worldOffsetA, ref linear, out taImpulse);
connectionA.ApplyAngularImpulse(ref taImpulse);
}
if (connectionB.isDynamic)
{
connectionB.ApplyLinearImpulse(ref lambda);
Vector3 tbImpulse;
Vector3.Cross(ref worldOffsetB, ref lambda, out tbImpulse);
connectionB.ApplyAngularImpulse(ref tbImpulse);
}
return(Math.Abs(lambda.X) +
Math.Abs(lambda.Y) +
Math.Abs(lambda.Z));
}
/// <summary>
/// Rotates the camera around its locked up vector.
/// </summary>
/// <param name="radians">Amount to rotate.</param>
public void Yaw(float radians)
{
//Rotate around the up vector.
Matrix3x3 rotation;
Matrix3x3.CreateFromAxisAngle(ref lockedUp, radians, out rotation);
Matrix3x3.Transform(ref viewDirection, ref rotation, out viewDirection);
//Avoid drift by renormalizing.
viewDirection.Normalize();
}
void AddLocalContact(ref ContactData contact, ref Matrix3x3 orientation, ref QuickList <ContactData> candidatesToAdd)
{
//Put the contact into world space.
Matrix3x3.Transform(ref contact.Position, ref orientation, out contact.Position);
Vector3.Add(ref contact.Position, ref convex.worldTransform.Position, out contact.Position);
Matrix3x3.Transform(ref contact.Normal, ref orientation, out contact.Normal);
//Check to see if the contact is unique before proceeding.
if (IsContactUnique(ref contact, ref candidatesToAdd))
{
candidatesToAdd.Add(ref contact);
}
}
public override void Update(float dt)
{
base.Update(dt);
Vector3 offset = TransformOffset ? Matrix3x3.Transform(OffsetFromChaseTarget, ChasedEntity.BufferedStates.InterpolatedStates.OrientationMatrix) : OffsetFromChaseTarget;
Vector3 lookAt = ChasedEntity.BufferedStates.InterpolatedStates.Position + offset;
Vector3 backwards = -Camera.ViewDirection;
//Find the earliest ray hit that isn't the chase target to position the camera appropriately.
RayCastResult result;
float cameraDistance = ChasedEntity.Space.RayCast(new Ray(lookAt, backwards), DistanceToTarget, rayCastFilter, out result) ? result.HitData.T : DistanceToTarget;
Camera.Position = lookAt + (Math.Max(cameraDistance - ChaseCameraMargin, 0)) * backwards; //Put the camera just before any hit spot.
}
internal void PreStep(float dt)
{
Matrix.CreateFromAxisAngle(ref suspension.localDirection, shape.steeringAngle, out shape.steeringTransform);
Matrix.TransformNormal(ref localForwardDirection, ref shape.steeringTransform, out worldForwardDirection);
Matrix3x3.Transform(ref worldForwardDirection, ref Vehicle.Body.orientationMatrix, out worldForwardDirection);
if (HasSupport)
{
Vector3.Subtract(ref supportLocation, ref Vehicle.Body.position, out ra);
if (supportingEntity != null)
{
Vector3.Subtract(ref supportLocation, ref SupportingEntity.position, out rb);
}
//Mind the order of updating! sliding friction must come before driving force or rolling friction
//because it computes the sliding direction.
suspension.isActive = true;
suspension.numIterationsAtZeroImpulse = 0;
suspension.solverSettings.currentIterations = 0;
slidingFriction.isActive = true;
slidingFriction.numIterationsAtZeroImpulse = 0;
slidingFriction.solverSettings.currentIterations = 0;
drivingMotor.isActive = true;
drivingMotor.numIterationsAtZeroImpulse = 0;
drivingMotor.solverSettings.currentIterations = 0;
brake.isActive = true;
brake.numIterationsAtZeroImpulse = 0;
brake.solverSettings.currentIterations = 0;
suspension.PreStep(dt);
slidingFriction.PreStep(dt);
drivingMotor.PreStep(dt);
brake.PreStep(dt);
}
else
{
//No support, don't need any solving.
suspension.isActive = false;
slidingFriction.isActive = false;
drivingMotor.isActive = false;
brake.isActive = false;
suspension.accumulatedImpulse = 0;
slidingFriction.accumulatedImpulse = 0;
drivingMotor.accumulatedImpulse = 0;
brake.accumulatedImpulse = 0;
}
}
/// <summary>
/// Updates the upright constraint.
/// Called automatically by its owning space.
/// </summary>
/// <param name="dt">Time since last frame in simulation seconds.</param>
void IDuringForcesUpdateable.Update(float dt)
{
myWorldUpVector = Matrix3x3.Transform(myLocalUpVector, Entity.OrientationMatrix);
//Compute the axis and angle
Vector3 axis = Vector3.Cross(myWorldUpVector, Vector3.Up);
var angle = (float)Math.Acos(Vector3.Dot(Vector3.Up, myWorldUpVector));
if (angle > MinimumAngle && angle < MaximumAngle)
{
angle = angle - MinimumAngle;
axis.Normalize();
Entity.AngularMomentum += (axis * (angle * CorrectionFactor * dt));
}
}
protected internal override int FindOverlappingTriangles(float dt)
{
BoundingBox boundingBox;
convex.Shape.GetLocalBoundingBox(ref convex.worldTransform, ref terrain.worldTransform, out boundingBox);
if (convex.entity != null)
{
Vector3 transformedVelocity;
Matrix3x3 inverse;
Matrix3x3.Invert(ref terrain.worldTransform.LinearTransform, out inverse);
Matrix3x3.Transform(ref convex.entity.linearVelocity, ref inverse, out transformedVelocity);
Vector3.Multiply(ref transformedVelocity, dt, out transformedVelocity);
if (transformedVelocity.X > 0)
{
boundingBox.Max.X += transformedVelocity.X;
}
else
{
boundingBox.Min.X += transformedVelocity.X;
}
if (transformedVelocity.Y > 0)
{
boundingBox.Max.Y += transformedVelocity.Y;
}
else
{
boundingBox.Min.Y += transformedVelocity.Y;
}
if (transformedVelocity.Z > 0)
{
boundingBox.Max.Z += transformedVelocity.Z;
}
else
{
boundingBox.Min.Z += transformedVelocity.Z;
}
}
terrain.Shape.GetOverlaps(boundingBox, ref overlappedTriangles);
return(overlappedTriangles.Count);
}
/// <summary>
/// Computes one iteration of the constraint to meet the solver updateable's goal.
/// </summary>
/// <returns>The rough applied impulse magnitude.</returns>
public override float SolveIteration()
{
//Compute relative velocity
Vector3 lambda;
Vector3.Cross(ref r, ref entity.angularVelocity, out lambda);
Vector3.Subtract(ref lambda, ref entity.linearVelocity, out lambda);
//Add in bias velocity
Vector3.Add(ref biasVelocity, ref lambda, out lambda);
//Add in softness
Vector3 softnessVelocity;
Vector3.Multiply(ref accumulatedImpulse, usedSoftness, out softnessVelocity);
Vector3.Subtract(ref lambda, ref softnessVelocity, out lambda);
//In terms of an impulse (an instantaneous change in momentum), what is it?
Matrix3x3.Transform(ref lambda, ref effectiveMassMatrix, out lambda);
//Sum the impulse.
Vector3 previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse += lambda;
//If the impulse it takes to get to the goal is too high for the motor to handle, scale it back.
float sumImpulseLengthSquared = accumulatedImpulse.LengthSquared();
if (sumImpulseLengthSquared > maxForceDtSquared)
{
//max / impulse gives some value 0 < x < 1. Basically, normalize the vector (divide by the length) and scale by the maximum.
accumulatedImpulse *= maxForceDt / (float)Math.Sqrt(sumImpulseLengthSquared);
//Since the limit was exceeded by this corrective impulse, limit it so that the accumulated impulse remains constrained.
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
entity.ApplyLinearImpulse(ref lambda);
Vector3 taImpulse;
Vector3.Cross(ref r, ref lambda, out taImpulse);
entity.ApplyAngularImpulse(ref taImpulse);
return(Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z));
}
public void Update(float dt)
{
//Only turn if the mouse is controlled by the game.
if (!Game.IsMouseVisible)
{
Camera.Yaw((200 - Game.MouseInput.X) * dt * .12f);
Camera.Pitch((200 - Game.MouseInput.Y) * dt * .12f);
}
Vector3 offset = TransformOffset ? Matrix3x3.Transform(OffsetFromChaseTarget, ChasedEntity.BufferedStates.InterpolatedStates.OrientationMatrix) : OffsetFromChaseTarget;
Vector3 lookAt = ChasedEntity.BufferedStates.InterpolatedStates.Position + offset;
Vector3 backwards = -Camera.ViewDirection;
//Find the earliest ray hit that isn't the chase target to position the camera appropriately.
RayCastResult result;
float cameraDistance = ChasedEntity.Space.RayCast(new Ray(lookAt, backwards), DistanceToTarget, rayCastFilter, out result) ? result.HitData.T : DistanceToTarget;
Camera.Position = lookAt + (Math.Max(cameraDistance - ChaseCameraMargin, 0)) * backwards; //Put the camera just before any hit spot.
}
public override void Update()
{
if (DisplayedObject.Entity != null)
{
//The reason for this complexity is that we're drawing the shape's location directly and interpolated buffers might be active.
//That means we can't rely solely on the collidable's world transform or the entity's world transform alone;
//we must rebuild it from the entity's world transform and the collidable's local position.
//TODO: This is awfully annoying. Could use some built-in convenience methods to ease the usage.
Vector3 translation = Matrix3x3.Transform(DisplayedObject.LocalPosition, DisplayedObject.Entity.BufferedStates.InterpolatedStates.OrientationMatrix);
translation += DisplayedObject.Entity.BufferedStates.InterpolatedStates.Position;
Matrix worldTransform = Matrix3x3.ToMatrix4X4(DisplayedObject.Entity.BufferedStates.InterpolatedStates.OrientationMatrix);
worldTransform.Translation = translation;
WorldTransform = MathConverter.Convert(worldTransform);
}
else
{
//Entityless EntityCollidables just go by what their current transform is.
WorldTransform = MathConverter.Convert(DisplayedObject.WorldTransform.Matrix);
}
}
///<summary>
/// Tests a ray against the instance.
///</summary>
///<param name="ray">Ray to test.</param>
///<param name="maximumLength">Maximum length of the ray to test; in units of the ray's direction's length.</param>
///<param name="sidedness">Sidedness to use during the ray cast. This does not have to be the same as the mesh's sidedness.</param>
///<param name="rayHit">The hit location of the ray on the mesh, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, out RayHit rayHit)
{
//Put the ray into local space.
Ray localRay;
AffineTransform inverse;
AffineTransform.Invert(ref worldTransform, out inverse);
Matrix3x3.Transform(ref ray.Direction, ref inverse.LinearTransform, out localRay.Direction);
AffineTransform.Transform(ref ray.Position, ref inverse, out localRay.Position);
if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit))
{
//Transform the hit into world space.
Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
Matrix3x3.TransformTranspose(ref rayHit.Normal, ref inverse.LinearTransform, out rayHit.Normal);
return(true);
}
rayHit = new RayHit();
return(false);
}
/// <summary>
/// Called automatically by the space.
/// </summary>
/// <param name="dt">Simulation timestep.</param>
void IDuringForcesUpdateable.Update(float dt)
{
if (Entity != LinearMotor.Entity)
{
throw new InvalidOperationException(
"EntityMover's entity differs from EntityMover's motors' entities. Ensure that the moved entity is only changed by setting the EntityMover's entity property.");
}
if (Entity.IsDynamic)
{
LinearMotor.IsActive = true;
LinearMotor.Settings.Servo.Goal = TargetPosition;
}
else
{
LinearMotor.IsActive = false;
Vector3 worldMovedPoint = Matrix3x3.Transform(LocalOffset, entity.orientationMatrix);
Vector3.Add(ref worldMovedPoint, ref entity.position, out worldMovedPoint);
Entity.LinearVelocity = GetLinearVelocity(worldMovedPoint, TargetPosition, dt);
}
}
/// <summary>
/// Applies the corrective impulses required by the constraint.
/// </summary>
public override float SolveIteration()
{
#if !WINDOWS
Vector3 lambda = new Vector3();
#else
Vector3 lambda;
#endif
Vector3 aVel = entity.angularVelocity;
lambda.X = -aVel.X + biasVelocity.X - usedSoftness * accumulatedImpulse.X;
lambda.Y = -aVel.Y + biasVelocity.Y - usedSoftness * accumulatedImpulse.Y;
lambda.Z = -aVel.Z + biasVelocity.Z - usedSoftness * accumulatedImpulse.Z;
Matrix3x3.Transform(ref lambda, ref effectiveMassMatrix, out lambda);
Vector3 previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse.X += lambda.X;
accumulatedImpulse.Y += lambda.Y;
accumulatedImpulse.Z += lambda.Z;
float sumLengthSquared = accumulatedImpulse.LengthSquared();
if (sumLengthSquared > maxForceDtSquared)
{
//max / impulse gives some value 0 < x < 1. Basically, normalize the vector (divide by the length) and scale by the maximum.
float multiplier = maxForceDt / (float)Math.Sqrt(sumLengthSquared);
accumulatedImpulse.X *= multiplier;
accumulatedImpulse.Y *= multiplier;
accumulatedImpulse.Z *= multiplier;
//Since the limit was exceeded by this corrective impulse, limit it so that the accumulated impulse remains constrained.
lambda.X = accumulatedImpulse.X - previousAccumulatedImpulse.X;
lambda.Y = accumulatedImpulse.Y - previousAccumulatedImpulse.Y;
lambda.Z = accumulatedImpulse.Z - previousAccumulatedImpulse.Z;
}
entity.ApplyAngularImpulse(ref lambda);
return(Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z));
}
///<summary>
/// Tests a ray against the surface of the mesh. This does not take into account solidity.
///</summary>
///<param name="ray">Ray to test.</param>
///<param name="maximumLength">Maximum length of the ray to test; in units of the ray's direction's length.</param>
///<param name="sidedness">Sidedness to use during the ray cast. This does not have to be the same as the mesh's sidedness.</param>
///<param name="rayHit">The hit location of the ray on the mesh, if any.</param>
///<returns>Whether or not the ray hit the mesh.</returns>
public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, out RayHit rayHit)
{
//Put the ray into local space.
Ray localRay;
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out orientation);
Matrix3x3.TransformTranspose(ref ray.Direction, ref orientation, out localRay.Direction);
Vector3.Subtract(ref ray.Position, ref worldTransform.Position, out localRay.Position);
Matrix3x3.TransformTranspose(ref localRay.Position, ref orientation, out localRay.Position);
if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit))
{
//Transform the hit into world space.
Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
return(true);
}
rayHit = new RayHit();
return(false);
}
void IDuringForcesUpdateable.Update(float dt)
{
if (linearMotor != null && entity != linearMotor.Entity)
{
throw new InvalidOperationException("EntityMover's entity differs from EntityMover's motors' entities.");
}
if (entity.IsDynamic)
{
if (linearMotor != null)
{
linearMotor.IsActive = true;
linearMotor.Settings.Servo.Goal = targetPosition;
}
}
else
{
if (linearMotor != null)
{
linearMotor.IsActive = false;
Vector3 worldMovedPoint = Matrix3x3.Transform(LocalOffset, entity.orientationMatrix);
}
}
}
public override void Update(float dt)
{
base.Update(dt);
////Rotate the camera of the character based on the support velocity, if a support with velocity exists.
////This can be very disorienting in some cases; that's why it is off by default!
//if (Character.SupportFinder.HasSupport)
//{
// SupportData? data;
// if (Character.SupportFinder.HasTraction)
// data = Character.SupportFinder.TractionData;
// else
// data = Character.SupportFinder.SupportData;
// var support = data.Value.SupportObject as EntityCollidable;
// if (support != null && !support.Entity.IsDynamic) //Having the view turned by dynamic entities is extremely confusing for the most part.
// {
// float dot = Vector3.Dot(support.Entity.AngularVelocity, Character.Body.OrientationMatrix.Up);
// Camera.Yaw(dot * dt);
// }
//}
Camera.Position = Entity.Position + Matrix3x3.Transform(CameraOffset, Entity.OrientationMatrix);
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
Vector3 a, b, c;
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
Matrix3x3.Transform(ref vA, ref o, out a);
Matrix3x3.Transform(ref vB, ref o, out b);
Matrix3x3.Transform(ref vC, ref o, out c);
Vector3.Min(ref a, ref b, out boundingBox.Min);
Vector3.Min(ref c, ref boundingBox.Min, out boundingBox.Min);
Vector3.Max(ref a, ref b, out boundingBox.Max);
Vector3.Max(ref c, ref boundingBox.Max, out boundingBox.Max);
boundingBox.Min.X += shapeTransform.Position.X - collisionMargin;
boundingBox.Min.Y += shapeTransform.Position.Y - collisionMargin;
boundingBox.Min.Z += shapeTransform.Position.Z - collisionMargin;
boundingBox.Max.X += shapeTransform.Position.X + collisionMargin;
boundingBox.Max.Y += shapeTransform.Position.Y + collisionMargin;
boundingBox.Max.Z += shapeTransform.Position.Z + collisionMargin;
}
/// <summary>
/// Calculates necessary information for velocity solving.
/// Called by preStep(float dt)
/// </summary>
/// <param name="dt">Time in seconds since the last update.</param>
public override void Update(float dt)
{
Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA);
Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB);
float errorReductionParameter;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReductionParameter, out softness);
//Mass Matrix
Matrix3x3 k;
Matrix3x3 linearComponent;
Matrix3x3.CreateCrossProduct(ref worldOffsetA, out rACrossProduct);
Matrix3x3.CreateCrossProduct(ref worldOffsetB, out rBCrossProduct);
if (connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3x3.CreateScale(connectionA.inverseMass + connectionB.inverseMass, out linearComponent);
Matrix3x3 angularComponentA, angularComponentB;
Matrix3x3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA);
Matrix3x3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB);
Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
Matrix3x3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB);
Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out k);
Matrix3x3.Subtract(ref k, ref angularComponentB, out k);
}
else if (connectionA.isDynamic && !connectionB.isDynamic)
{
Matrix3x3.CreateScale(connectionA.inverseMass, out linearComponent);
Matrix3x3 angularComponentA;
Matrix3x3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA);
Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out k);
}
else if (!connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3x3.CreateScale(connectionB.inverseMass, out linearComponent);
Matrix3x3 angularComponentB;
Matrix3x3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB);
Matrix3x3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB);
Matrix3x3.Subtract(ref linearComponent, ref angularComponentB, out k);
}
else
{
throw new InvalidOperationException("Cannot constrain two kinematic bodies.");
}
k.M11 += softness;
k.M22 += softness;
k.M33 += softness;
Matrix3x3.Invert(ref k, out massMatrix);
Vector3.Add(ref connectionB.position, ref worldOffsetB, out error);
Vector3.Subtract(ref error, ref connectionA.position, out error);
Vector3.Subtract(ref error, ref worldOffsetA, out error);
Vector3.Multiply(ref error, -errorReductionParameter, out biasVelocity);
//Ensure that the corrective velocity doesn't exceed the max.
float length = biasVelocity.LengthSquared();
if (length > maxCorrectiveVelocitySquared)
{
float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length);
biasVelocity.X *= multiplier;
biasVelocity.Y *= multiplier;
biasVelocity.Z *= multiplier;
}
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
Matrix3x3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA);
Matrix3x3.Transform(ref localAxisB, ref connectionB.orientationMatrix, out worldAxisB);
Matrix3x3.Transform(ref localConstrainedAxis1, ref connectionA.orientationMatrix, out worldConstrainedAxis1);
Matrix3x3.Transform(ref localConstrainedAxis2, ref connectionA.orientationMatrix, out worldConstrainedAxis2);
Vector3 error;
Vector3.Cross(ref worldAxisA, ref worldAxisB, out error);
Vector3.Dot(ref error, ref worldConstrainedAxis1, out this.error.X);
Vector3.Dot(ref error, ref worldConstrainedAxis2, out this.error.Y);
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);
errorReduction = -errorReduction;
biasVelocity.X = errorReduction * this.error.X;
biasVelocity.Y = errorReduction * this.error.Y;
//Ensure that the corrective velocity doesn't exceed the max.
float length = biasVelocity.LengthSquared();
if (length > maxCorrectiveVelocitySquared)
{
float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length);
biasVelocity.X *= multiplier;
biasVelocity.Y *= multiplier;
}
Vector3 axis1I, axis2I;
if (connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3x3 inertiaTensorSum;
Matrix3x3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out inertiaTensorSum);
Matrix3x3.Transform(ref worldConstrainedAxis1, ref inertiaTensorSum, out axis1I);
Matrix3x3.Transform(ref worldConstrainedAxis2, ref inertiaTensorSum, out axis2I);
}
else if (connectionA.isDynamic && !connectionB.isDynamic)
{
Matrix3x3.Transform(ref worldConstrainedAxis1, ref connectionA.inertiaTensorInverse, out axis1I);
Matrix3x3.Transform(ref worldConstrainedAxis2, ref connectionA.inertiaTensorInverse, out axis2I);
}
else if (!connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3x3.Transform(ref worldConstrainedAxis1, ref connectionB.inertiaTensorInverse, out axis1I);
Matrix3x3.Transform(ref worldConstrainedAxis2, ref connectionB.inertiaTensorInverse, out axis2I);
}
else
{
throw new InvalidOperationException("Cannot constrain two kinematic bodies.");
}
Vector3.Dot(ref axis1I, ref worldConstrainedAxis1, out effectiveMassMatrix.M11);
Vector3.Dot(ref axis1I, ref worldConstrainedAxis2, out effectiveMassMatrix.M12);
Vector3.Dot(ref axis2I, ref worldConstrainedAxis1, out effectiveMassMatrix.M21);
Vector3.Dot(ref axis2I, ref worldConstrainedAxis2, out effectiveMassMatrix.M22);
effectiveMassMatrix.M11 += softness;
effectiveMassMatrix.M22 += softness;
Matrix2x2.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
Matrix2x2.Negate(ref effectiveMassMatrix, out effectiveMassMatrix);
}
/// <summary>
/// Tests a ray against the entry.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="maximumLength">Maximum length, in units of the ray's direction's length, to test.</param>
/// <param name="rayHit">Hit location of the ray on the entry, if any.</param>
/// <returns>Whether or not the ray hit the entry.</returns>
public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit)
{
//Put the ray into local space.
Ray localRay;
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out orientation);
Matrix3x3.TransformTranspose(ref ray.Direction, ref orientation, out localRay.Direction);
Vector3.Subtract(ref ray.Position, ref worldTransform.Position, out localRay.Position);
Matrix3x3.TransformTranspose(ref localRay.Position, ref orientation, out localRay.Position);
if (Shape.solidity == MobileMeshSolidity.Solid)
{
//Find all hits. Use the count to determine the ray started inside or outside.
//If it starts inside and we're in 'solid' mode, then return the ray start.
//The raycast must be of infinite length at first. This allows it to determine
//if it is inside or outside.
if (Shape.IsLocalRayOriginInMesh(ref localRay, out rayHit))
{
//It was inside!
rayHit = new RayHit()
{
Location = ray.Position, Normal = Vector3.Zero, T = 0
};
return(true);
}
else
{
if (rayHit.T < maximumLength)
{
//Transform the hit into world space.
Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
}
else
{
//The hit was too far away, or there was no hit (in which case T would be float.MaxValue).
return(false);
}
return(true);
}
}
else
{
//Just do a normal raycast since the object isn't solid.
TriangleSidedness sidedness;
switch (Shape.solidity)
{
case MobileMeshSolidity.Clockwise:
sidedness = TriangleSidedness.Clockwise;
break;
case MobileMeshSolidity.Counterclockwise:
sidedness = TriangleSidedness.Counterclockwise;
break;
default:
sidedness = TriangleSidedness.DoubleSided;
break;
}
if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit))
{
//Transform the hit into world space.
Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
return(true);
}
}
rayHit = new RayHit();
return(false);
}
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);
}
internal void ComputeWorldSpaceAxes()
{
Matrix3x3.Transform(ref localPrimaryAxis, ref rotationMatrix, out primaryAxis);
Matrix3x3.Transform(ref localXAxis, ref rotationMatrix, out xAxis);
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//Transform the axes into world space.
basis.rotationMatrix = connectionA.orientationMatrix;
basis.ComputeWorldSpaceAxes();
Matrix3x3.Transform(ref localTestAxis, ref connectionB.orientationMatrix, out worldTestAxis);
float updateRate = 1 / dt;
if (settings.mode == MotorMode.Servomechanism)
{
float y, x;
Vector3 yAxis;
Vector3.Cross(ref basis.primaryAxis, ref Basis.xAxis, out yAxis);
Vector3.Dot(ref worldTestAxis, ref yAxis, out y);
Vector3.Dot(ref worldTestAxis, ref Basis.xAxis, out x);
var angle = (float)Math.Atan2(y, x);
//****** VELOCITY BIAS ******//
//Compute the correction velocity.
error = GetDistanceFromGoal(angle);
float absErrorOverDt = Math.Abs(error * updateRate);
float errorReduction;
settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, updateRate, 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 * updateRate;
error = 0;
}
//Compute the jacobians
jacobianA = basis.primaryAxis;
jacobianB.X = -jacobianA.X;
jacobianB.Y = -jacobianA.Y;
jacobianB.Z = -jacobianA.Z;
//****** 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);
//Update the maximum force
ComputeMaxForces(settings.maximumForce, dt);
}
/// <summary>
/// Initializes the constraint for this frame.
/// </summary>
/// <param name="dt">Time since the last frame.</param>
public override void Update(float dt)
{
Matrix3x3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA);
Matrix3x3.Transform(ref localAxisB, ref connectionB.orientationMatrix, out worldAxisB);
float dot;
Vector3.Dot(ref worldAxisA, ref worldAxisB, out dot);
//Keep in mind, the dot is the cosine of the angle.
//1: 0 radians
//0: pi/2 radians
//-1: pi radians
if (dot > minimumCosine)
{
isActiveInSolver = false;
error = 0;
accumulatedImpulse = 0;
isLimitActive = false;
return;
}
isLimitActive = true;
//Hinge axis is actually the jacobian entry for angular A (negative angular B).
Vector3.Cross(ref worldAxisA, ref worldAxisB, out hingeAxis);
float lengthSquared = hingeAxis.LengthSquared();
if (lengthSquared < Toolbox.Epsilon)
{
//They're parallel; for the sake of continuity, pick some axis which is perpendicular to both that ISN'T the zero vector.
Vector3.Cross(ref worldAxisA, ref Toolbox.UpVector, out hingeAxis);
lengthSquared = hingeAxis.LengthSquared();
if (lengthSquared < Toolbox.Epsilon)
{
//That's improbable; b's world axis was apparently parallel with the up vector!
//So just use the right vector (it can't be parallel with both the up and right vectors).
Vector3.Cross(ref worldAxisA, ref Toolbox.RightVector, out hingeAxis);
}
}
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);
//Further away from 0 degrees is further negative; if the dot is below the minimum cosine, it means the angle is above the maximum angle.
error = Math.Max(0, minimumCosine - dot - margin);
biasVelocity = MathHelper.Clamp(errorReduction * error, -maxCorrectiveVelocity, maxCorrectiveVelocity);
if (bounciness > 0)
{
//Compute the speed around the axis.
float relativeSpeed;
Vector3 relativeVelocity;
Vector3.Subtract(ref connectionA.angularVelocity, ref connectionB.angularVelocity, out relativeVelocity);
Vector3.Dot(ref relativeVelocity, ref hingeAxis, out relativeSpeed);
biasVelocity = MathHelper.Max(biasVelocity, ComputeBounceVelocity(-relativeSpeed));
}
//Connection A's contribution to the mass matrix
float entryA;
Vector3 transformedAxis;
if (connectionA.isDynamic)
{
Matrix3x3.Transform(ref hingeAxis, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref hingeAxis, out entryA);
}
else
{
entryA = 0;
}
//Connection B's contribution to the mass matrix
float entryB;
if (connectionB.isDynamic)
{
Matrix3x3.Transform(ref hingeAxis, ref connectionB.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref hingeAxis, out entryB);
}
else
{
entryB = 0;
}
//Compute the inverse mass matrix
velocityToImpulse = 1 / (softness + entryA + entryB);
}
/// <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)
{
Vector3 aAxisY, aAxisZ;
Vector3 bAxisY;
Matrix3x3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA);
Matrix3x3.Transform(ref aLocalAxisY, ref connectionA.orientationMatrix, out aAxisY);
Matrix3x3.Transform(ref aLocalAxisZ, ref connectionA.orientationMatrix, out aAxisZ);
Matrix3x3.Transform(ref localAxisB, ref connectionB.orientationMatrix, out worldAxisB);
Matrix3x3.Transform(ref bLocalAxisY, ref connectionB.orientationMatrix, out bAxisY);
Quaternion rotation;
Quaternion.GetQuaternionBetweenNormalizedVectors(ref worldAxisB, ref worldAxisA, out rotation);
//Transform b's 'Y' axis so that it is perpendicular with a's 'X' axis for measurement.
Vector3 twistMeasureAxis;
Quaternion.Transform(ref bAxisY, 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 aAxisZ, out y);
Vector3.Dot(ref twistMeasureAxis, ref aAxisY, out x);
error = (float)Math.Atan2(y, x);
//Debug.WriteLine("Angle: " + angle);
//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 worldAxisA, ref worldAxisB, 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;
//****** VELOCITY BIAS ******//
//Compute the correction velocity.
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);
biasVelocity = MathHelper.Clamp(-error * errorReduction, -maxCorrectiveVelocity, maxCorrectiveVelocity);
//****** 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 / (softness + entryA + entryB);
}
public override void GetMeshData(List <VertexPositionNormalTexture> vertices, List <ushort> indices)
{
int numColumns = DisplayedObject.Shape.Heights.GetLength(0);
int numRows = DisplayedObject.Shape.Heights.GetLength(1);
TerrainShape shape = DisplayedObject.Shape;
//The terrain can be transformed arbitrarily. However, the collision against the triangles is always oriented such that the transformed local
//up vector points in the same direction as the collidable surfaces.
//To make sure the graphics match the terrain collision, try transforming the local space up direction into world space. Treat it as a normal- it requires an adjugate transpose, not a regular transformation.
var normalTransform = Matrix3x3.AdjugateTranspose(DisplayedObject.WorldTransform.LinearTransform);
var reverseWinding = Vector3.Dot(normalTransform.Up, DisplayedObject.WorldTransform.LinearTransform.Up) < 0;
for (int j = 0; j < numRows; j++)
{
for (int i = 0; i < numColumns; i++)
{
VertexPositionNormalTexture v;
Vector3 position, n;
DisplayedObject.GetPosition(i, j, out position);
shape.GetLocalNormal(i, j, out n);
Matrix3x3.Transform(ref n, ref normalTransform, out n);
n.Normalize();
MathConverter.Convert(ref position, out v.Position);
MathConverter.Convert(ref n, out v.Normal);
if (reverseWinding)
{
Microsoft.Xna.Framework.Vector3.Negate(ref v.Normal, out v.Normal);
}
v.TextureCoordinate = new Microsoft.Xna.Framework.Vector2(i, j);
vertices.Add(v);
if (i < numColumns - 1 && j < numRows - 1)
{
if (shape.QuadTriangleOrganization == QuadTriangleOrganization.BottomLeftUpperRight)
{
//v3 v4
//v1 v2
//v1 v2 v3
indices.Add((ushort)(numColumns * j + i));
if (reverseWinding)
{
indices.Add((ushort)(numColumns * (j + 1) + i));
indices.Add((ushort)(numColumns * j + i + 1));
}
else
{
indices.Add((ushort)(numColumns * j + i + 1));
indices.Add((ushort)(numColumns * (j + 1) + i));
}
//v2 v4 v3
indices.Add((ushort)(numColumns * j + i + 1));
if (reverseWinding)
{
indices.Add((ushort)(numColumns * (j + 1) + i));
indices.Add((ushort)(numColumns * (j + 1) + i + 1));
}
else
{
indices.Add((ushort)(numColumns * (j + 1) + i + 1));
indices.Add((ushort)(numColumns * (j + 1) + i));
}
}
else if (shape.QuadTriangleOrganization == QuadTriangleOrganization.BottomRightUpperLeft)
{
//v1 v2 v4
indices.Add((ushort)(numColumns * j + i));
if (reverseWinding)
{
indices.Add((ushort)(numColumns * (j + 1) + i + 1));
indices.Add((ushort)(numColumns * j + i + 1));
}
else
{
indices.Add((ushort)(numColumns * j + i + 1));
indices.Add((ushort)(numColumns * (j + 1) + i + 1));
}
//v1 v4 v3
indices.Add((ushort)(numColumns * j + i));
if (reverseWinding)
{
indices.Add((ushort)(numColumns * (j + 1) + i));
indices.Add((ushort)(numColumns * (j + 1) + i + 1));
}
else
{
indices.Add((ushort)(numColumns * (j + 1) + i + 1));
indices.Add((ushort)(numColumns * (j + 1) + i));
}
}
}
}
}
}