/// <summary>
/// Constructs a new constraint which restricts three degrees of linear freedom and two degrees of angular freedom between two entities.
/// </summary>
/// <param name="connectionA">First entity of the constraint pair.</param>
/// <param name="connectionB">Second entity of the constraint pair.</param>
/// <param name="anchor">Point around which both entities rotate.</param>
/// <param name="freeAxis">Axis around which the hinge can rotate.</param>
public RevoluteJoint(Entity connectionA, Entity connectionB, System.Numerics.Vector3 anchor, System.Numerics.Vector3 freeAxis)
{
if (connectionA == null)
{
connectionA = TwoEntityConstraint.WorldEntity;
}
if (connectionB == null)
{
connectionB = TwoEntityConstraint.WorldEntity;
}
BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor);
AngularJoint = new RevoluteAngularJoint(connectionA, connectionB, freeAxis);
Limit = new RevoluteLimit(connectionA, connectionB);
Motor = new RevoluteMotor(connectionA, connectionB, freeAxis);
Limit.IsActive = false;
Motor.IsActive = false;
//Ensure that the base and test direction is perpendicular to the free axis.
System.Numerics.Vector3 baseAxis = anchor - connectionA.position;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way.
{
baseAxis = connectionB.position - anchor;
}
baseAxis -= Vector3Ex.Dot(baseAxis, freeAxis) * freeAxis;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
//However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Right);
}
}
Limit.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix);
Motor.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix);
baseAxis = connectionB.position - anchor;
baseAxis -= Vector3Ex.Dot(baseAxis, freeAxis) * freeAxis;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
//However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Right);
}
}
Limit.TestAxis = baseAxis;
Motor.TestAxis = baseAxis;
Add(BallSocketJoint);
Add(AngularJoint);
Add(Limit);
Add(Motor);
}
public static void Validate(this System.Numerics.Vector3 v)
{
if (IsInvalid(v.LengthSquared()))
{
throw new NotFiniteNumberException("Invalid value.");
}
}
void IForceUpdateable.UpdateForForces(float dt)
{
//Linear velocity
if (IsAffectedByGravity)
{
Vector3Ex.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)
{
Vector3Ex.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
Vector3Ex.Multiply(ref angularMomentum, (float)Math.Pow(MathHelper.Clamp(1 - angular, 0, 1), dt), out angularMomentum);
#else
Vector3Ex.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);
}
/// <summary>
/// Updates the movement basis of the horizontal motion constraint.
/// Should be updated automatically by the character on each time step; other code should not need to call this.
/// </summary>
/// <param name="forward">Forward facing direction of the character.</param>
public void UpdateMovementBasis(ref System.Numerics.Vector3 forward)
{
System.Numerics.Vector3 down = characterBody.orientationMatrix.Down;
System.Numerics.Vector3 strafeDirection;
System.Numerics.Vector3 horizontalForwardDirection = forward - down * Vector3Ex.Dot(down, forward);
float forwardLengthSquared = horizontalForwardDirection.LengthSquared();
if (forwardLengthSquared < Toolbox.Epsilon)
{
//Use an arbitrary direction to complete the basis.
horizontalForwardDirection = characterBody.orientationMatrix.Forward;
strafeDirection = characterBody.orientationMatrix.Right;
}
else
{
Vector3Ex.Divide(ref horizontalForwardDirection, (float)Math.Sqrt(forwardLengthSquared), out horizontalForwardDirection);
Vector3Ex.Cross(ref down, ref horizontalForwardDirection, out strafeDirection);
//Don't need to normalize the strafe direction; it's the cross product of two normalized perpendicular vectors.
}
Vector3Ex.Multiply(ref horizontalForwardDirection, movementDirection.Y, out movementDirection3d);
System.Numerics.Vector3 strafeComponent;
Vector3Ex.Multiply(ref strafeDirection, movementDirection.X, out strafeComponent);
Vector3Ex.Add(ref strafeComponent, ref movementDirection3d, out movementDirection3d);
}
///<summary>
/// Gets the error tolerance of the simplex.
///</summary>
///<returns>Error tolerance of the simplex.</returns>
public float GetErrorTolerance()
{
switch (State)
{
case SimplexState.Point:
return(A.LengthSquared());
case SimplexState.Segment:
return(MathHelper.Max(A.LengthSquared(), B.LengthSquared()));
case SimplexState.Triangle:
return(MathHelper.Max(A.LengthSquared(), MathHelper.Max(B.LengthSquared(), C.LengthSquared())));
case SimplexState.Tetrahedron:
return(MathHelper.Max(A.LengthSquared(), MathHelper.Max(B.LengthSquared(), MathHelper.Max(C.LengthSquared(), D.LengthSquared()))));
}
return(1);
}
private void UpdateRestrictedAxes()
{
localConstrainedAxis1 = System.Numerics.Vector3.Cross(Vector3Ex.Up, localAxisA);
if (localConstrainedAxis1.LengthSquared() < .001f)
{
localConstrainedAxis1 = System.Numerics.Vector3.Cross(Vector3Ex.Right, localAxisA);
}
localConstrainedAxis2 = System.Numerics.Vector3.Cross(localAxisA, localConstrainedAxis1);
localConstrainedAxis1.Normalize();
localConstrainedAxis2.Normalize();
}
///<summary>
/// Gets the extreme point of the shape in world space in a given direction with margin expansion.
///</summary>
///<param name="direction">Direction to find the extreme point in.</param>
/// <param name="shapeTransform">Transform to use for the shape.</param>
///<param name="extremePoint">Extreme point on the shape.</param>
public void GetExtremePoint(System.Numerics.Vector3 direction, ref RigidTransform shapeTransform, out System.Numerics.Vector3 extremePoint)
{
GetExtremePointWithoutMargin(direction, ref shapeTransform, out extremePoint);
float directionLength = direction.LengthSquared();
if (directionLength > Toolbox.Epsilon)
{
Vector3Ex.Multiply(ref direction, collisionMargin / (float)Math.Sqrt(directionLength), out direction);
Vector3Ex.Add(ref extremePoint, ref direction, out extremePoint);
}
}
protected internal override void SolveVelocityIteration()
{
//Compute the 'relative' linear and angular velocities. For single bone constraints, it's based entirely on the one bone's velocities!
//They have to be pulled into constraint space first to compute the necessary impulse, though.
System.Numerics.Vector3 linearContribution;
Matrix3x3.TransformTranspose(ref TargetBone.linearVelocity, ref linearJacobian, out linearContribution);
System.Numerics.Vector3 angularContribution;
Matrix3x3.TransformTranspose(ref TargetBone.angularVelocity, ref angularJacobian, out angularContribution);
//The constraint velocity error will be the velocity we try to remove.
System.Numerics.Vector3 constraintVelocityError;
Vector3Ex.Add(ref linearContribution, ref angularContribution, out constraintVelocityError);
//However, we need to take into account two extra sources of velocities which modify our target velocity away from zero.
//First, the velocity bias from position correction:
Vector3Ex.Subtract(ref constraintVelocityError, ref velocityBias, out constraintVelocityError);
//And second, the bias from softness:
System.Numerics.Vector3 softnessBias;
Vector3Ex.Multiply(ref accumulatedImpulse, -softness, out softnessBias);
Vector3Ex.Subtract(ref constraintVelocityError, ref softnessBias, out constraintVelocityError);
//By now, the constraint velocity error contains all the velocity we want to get rid of.
//Convert it into an impulse using the effective mass matrix.
System.Numerics.Vector3 constraintSpaceImpulse;
Matrix3x3.Transform(ref constraintVelocityError, ref effectiveMass, out constraintSpaceImpulse);
Vector3Ex.Negate(ref constraintSpaceImpulse, out constraintSpaceImpulse);
//Add the constraint space impulse to the accumulated impulse so that warm starting and softness work properly.
System.Numerics.Vector3 preadd = accumulatedImpulse;
Vector3Ex.Add(ref constraintSpaceImpulse, ref accumulatedImpulse, out accumulatedImpulse);
//But wait! The accumulated impulse may exceed this constraint's capacity! Check to make sure!
float impulseSquared = accumulatedImpulse.LengthSquared();
if (impulseSquared > maximumImpulseSquared)
{
//Oops! Clamp that down.
Vector3Ex.Multiply(ref accumulatedImpulse, maximumImpulse / (float)Math.Sqrt(impulseSquared), out accumulatedImpulse);
//Update the impulse based upon the clamped accumulated impulse and the original, pre-add accumulated impulse.
Vector3Ex.Subtract(ref accumulatedImpulse, ref preadd, out constraintSpaceImpulse);
}
//The constraint space impulse now represents the impulse we want to apply to the bone... but in constraint space.
//Bring it out to world space using the transposed jacobian.
System.Numerics.Vector3 linearImpulse;
Matrix3x3.Transform(ref constraintSpaceImpulse, ref linearJacobian, out linearImpulse);
System.Numerics.Vector3 angularImpulse;
Matrix3x3.Transform(ref constraintSpaceImpulse, ref angularJacobian, out angularImpulse);
//Apply them!
TargetBone.ApplyLinearImpulse(ref linearImpulse);
TargetBone.ApplyAngularImpulse(ref angularImpulse);
}
public void Update()
{
Velocity += _acceleration;
Position += Velocity;
_acceleration = System.Numerics.Vector3.Zero;
if (Velocity.LengthSquared() < 0.001f * 0.001f)
{
Velocity = System.Numerics.Vector3.Zero;
}
Velocity *= _damping;
_damping = 0.98f;
}
/// <summary>
/// Applies the corrective impulses required by the constraint.
/// </summary>
public override float SolveIteration()
{
#if !WINDOWS
System.Numerics.Vector3 lambda = new System.Numerics.Vector3();
#else
System.Numerics.Vector3 lambda;
#endif
System.Numerics.Vector3 aVel = connectionA.angularVelocity;
System.Numerics.Vector3 bVel = connectionB.angularVelocity;
lambda.X = bVel.X - aVel.X - biasVelocity.X - usedSoftness * accumulatedImpulse.X;
lambda.Y = bVel.Y - aVel.Y - biasVelocity.Y - usedSoftness * accumulatedImpulse.Y;
lambda.Z = bVel.Z - aVel.Z - biasVelocity.Z - usedSoftness * accumulatedImpulse.Z;
Matrix3x3.Transform(ref lambda, ref effectiveMassMatrix, out lambda);
System.Numerics.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;
}
if (connectionA.isDynamic)
{
connectionA.ApplyAngularImpulse(ref lambda);
}
if (connectionB.isDynamic)
{
System.Numerics.Vector3 torqueB;
Vector3Ex.Negate(ref lambda, out torqueB);
connectionB.ApplyAngularImpulse(ref torqueB);
}
return(Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z));
}
///<summary>
/// Computes the expansion of the minkowski sum due to margins in a given direction.
///</summary>
///<param name="marginA">First margin.</param>
///<param name="marginB">Second margin.</param>
///<param name="direction">Extreme point direction.</param>
///<param name="contribution">Margin contribution to the extreme point.</param>
public static void ExpandMinkowskiSum(float marginA, float marginB, ref System.Numerics.Vector3 direction, out System.Numerics.Vector3 contribution)
{
float lengthSquared = direction.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
//The contribution to the minkowski sum by the margin is:
//direction * marginA - (-direction) * marginB.
Vector3Ex.Multiply(ref direction, (marginA + marginB) / (float)Math.Sqrt(lengthSquared), out contribution);
}
else
{
contribution = new System.Numerics.Vector3();
}
}
/// <summary>
/// Calculates and applies corrective impulses.
/// Called automatically by space.
/// </summary>
public override float SolveIteration()
{
float linearSpeed = entity.linearVelocity.LengthSquared();
if (linearSpeed > maximumSpeedSquared)
{
linearSpeed = (float)Math.Sqrt(linearSpeed);
System.Numerics.Vector3 impulse;
//divide by linearSpeed to normalize the velocity.
//Multiply by linearSpeed - maximumSpeed to get the 'velocity change vector.'
Vector3Ex.Multiply(ref entity.linearVelocity, -(linearSpeed - maximumSpeed) / linearSpeed, out impulse);
//incorporate softness
System.Numerics.Vector3 softnessImpulse;
Vector3Ex.Multiply(ref accumulatedImpulse, usedSoftness, out softnessImpulse);
Vector3Ex.Subtract(ref impulse, ref softnessImpulse, out impulse);
//Transform into impulse
Vector3Ex.Multiply(ref impulse, effectiveMassMatrix, out impulse);
//Accumulate
System.Numerics.Vector3 previousAccumulatedImpulse = accumulatedImpulse;
Vector3Ex.Add(ref accumulatedImpulse, ref impulse, out accumulatedImpulse);
float forceMagnitude = accumulatedImpulse.LengthSquared();
if (forceMagnitude > 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(forceMagnitude);
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.
impulse.X = accumulatedImpulse.X - previousAccumulatedImpulse.X;
impulse.Y = accumulatedImpulse.Y - previousAccumulatedImpulse.Y;
impulse.Z = accumulatedImpulse.Z - previousAccumulatedImpulse.Z;
}
entity.ApplyLinearImpulse(ref impulse);
return(Math.Abs(impulse.X) + Math.Abs(impulse.Y) + Math.Abs(impulse.Z));
}
return(0);
}
///<summary>
/// Computes the expansion of the minkowski sum due to margins in a given direction.
///</summary>
///<param name="marginA">First margin.</param>
///<param name="marginB">Second margin.</param>
///<param name="direction">Extreme point direction.</param>
///<param name="toExpandA">Margin contribution to the shapeA.</param>
///<param name="toExpandB">Margin contribution to the shapeB.</param>
public static void ExpandMinkowskiSum(float marginA, float marginB, System.Numerics.Vector3 direction, ref System.Numerics.Vector3 toExpandA, ref System.Numerics.Vector3 toExpandB)
{
float lengthSquared = direction.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
lengthSquared = 1 / (float)Math.Sqrt(lengthSquared);
//The contribution to the minkowski sum by the margin is:
//direction * marginA - (-direction) * marginB.
System.Numerics.Vector3 contribution;
Vector3Ex.Multiply(ref direction, marginA * lengthSquared, out contribution);
Vector3Ex.Add(ref toExpandA, ref contribution, out toExpandA);
Vector3Ex.Multiply(ref direction, marginB * lengthSquared, out contribution);
Vector3Ex.Subtract(ref toExpandB, ref contribution, out toExpandB);
}
//If the direction is too small, then the expansion values are left unchanged.
}
/// <summary>
/// Initializes the constraint for the current frame.
/// </summary>
/// <param name="dt">Time between frames.</param>
public override void Update(float dt)
{
System.Numerics.Quaternion quaternionA;
QuaternionEx.Multiply(ref connectionA.orientation, ref initialQuaternionConjugateA, out quaternionA);
System.Numerics.Quaternion quaternionB;
QuaternionEx.Multiply(ref connectionB.orientation, ref initialQuaternionConjugateB, out quaternionB);
QuaternionEx.Conjugate(ref quaternionB, out quaternionB);
System.Numerics.Quaternion intermediate;
QuaternionEx.Multiply(ref quaternionA, ref quaternionB, out intermediate);
float angle;
System.Numerics.Vector3 axis;
QuaternionEx.GetAxisAngleFromQuaternion(ref intermediate, out axis, out angle);
error.X = axis.X * angle;
error.Y = axis.Y * angle;
error.Z = axis.Z * angle;
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);
errorReduction = -errorReduction;
biasVelocity.X = errorReduction * error.X;
biasVelocity.Y = errorReduction * error.Y;
biasVelocity.Z = errorReduction * error.Z;
//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;
}
Matrix3x3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out effectiveMassMatrix);
effectiveMassMatrix.M11 += softness;
effectiveMassMatrix.M22 += softness;
effectiveMassMatrix.M33 += softness;
Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
}
private static void SetHandbrakeAndBoost(ref Controller controller, ref Vector3 carLocation, System.Numerics.Vector3 carVelocity, double botFrontToTargetAngle)
{
if (Math.Abs(botFrontToTargetAngle) > DegreesToRadians(100) && Math.Sqrt(carVelocity.LengthSquared()) > 0.6)
{
controller.Handbrake = true;
controller.Boost = false;
}
if (carLocation.Z > 50)
{
controller.Handbrake = false;
}
if (Math.Abs(botFrontToTargetAngle) < MinimumSteeringAngleRadians)
{
controller.Boost = true;
}
}
/// <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
System.Numerics.Vector3 lambda;
Vector3Ex.Cross(ref r, ref entity.angularVelocity, out lambda);
Vector3Ex.Subtract(ref lambda, ref entity.linearVelocity, out lambda);
//Add in bias velocity
Vector3Ex.Add(ref biasVelocity, ref lambda, out lambda);
//Add in softness
System.Numerics.Vector3 softnessVelocity;
Vector3Ex.Multiply(ref accumulatedImpulse, usedSoftness, out softnessVelocity);
Vector3Ex.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.
System.Numerics.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);
System.Numerics.Vector3 taImpulse;
Vector3Ex.Cross(ref r, ref lambda, out taImpulse);
entity.ApplyAngularImpulse(ref taImpulse);
return(Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z));
}
protected void GetNormal(ref System.Numerics.Vector3 uncorrectedNormal, TriangleShape localTriangleShape, out System.Numerics.Vector3 normal)
{
//Compute the normal of the triangle in the current convex's local space.
//Note its reliance on the local triangle shape. It must be initialized to the correct values before this is called.
System.Numerics.Vector3 AB, AC;
Vector3Ex.Subtract(ref localTriangleShape.vB, ref localTriangleShape.vA, out AB);
Vector3Ex.Subtract(ref localTriangleShape.vC, ref localTriangleShape.vA, out AC);
//Compute the normal based on the sidedness.
switch (localTriangleShape.sidedness)
{
case TriangleSidedness.DoubleSided:
//If it's double sided, then pick the triangle normal which points in the same direction
//as the contact normal that's going to be corrected.
float dot;
Vector3Ex.Cross(ref AB, ref AC, out normal);
Vector3Ex.Dot(ref normal, ref uncorrectedNormal, out dot);
if (dot < 0)
{
Vector3Ex.Negate(ref normal, out normal);
}
break;
case TriangleSidedness.Clockwise:
//If it's clockwise, always use ACxAB.
Vector3Ex.Cross(ref AC, ref AB, out normal);
break;
default:
//If it's counterclockwise, always use ABxAC.
Vector3Ex.Cross(ref AB, ref AC, out normal);
break;
}
//If the normal is degenerate, just use the uncorrected normal.
if (normal.LengthSquared() < Toolbox.Epsilon)
{
normal = uncorrectedNormal;
}
}
void ComputeConstrainedAxes()
{
System.Numerics.Vector3 worldAxisA = WorldFreeAxisA;
System.Numerics.Vector3 error = System.Numerics.Vector3.Cross(worldAxisA, WorldFreeAxisB);
float lengthSquared = error.LengthSquared();
System.Numerics.Vector3 worldConstrainedAxis1, worldConstrainedAxis2;
//Find the first constrained axis.
if (lengthSquared > Toolbox.Epsilon)
{
//The error direction can be used as the first axis!
Vector3Ex.Divide(ref error, (float)Math.Sqrt(lengthSquared), out worldConstrainedAxis1);
}
else
{
//There's not enough error for it to be a good constrained axis.
//We'll need to create the constrained axes arbitrarily.
Vector3Ex.Cross(ref Toolbox.UpVector, ref worldAxisA, out worldConstrainedAxis1);
lengthSquared = worldConstrainedAxis1.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
//The up vector worked!
Vector3Ex.Divide(ref worldConstrainedAxis1, (float)Math.Sqrt(lengthSquared), out worldConstrainedAxis1);
}
else
{
//The up vector didn't work. Just try the right vector.
Vector3Ex.Cross(ref Toolbox.RightVector, ref worldAxisA, out worldConstrainedAxis1);
worldConstrainedAxis1.Normalize();
}
}
//Don't have to normalize the second constraint axis; it's the cross product of two perpendicular normalized vectors.
Vector3Ex.Cross(ref worldAxisA, ref worldConstrainedAxis1, out worldConstrainedAxis2);
localConstrainedAxis1 = QuaternionEx.Transform(worldConstrainedAxis1, QuaternionEx.Conjugate(ConnectionA.Orientation));
localConstrainedAxis2 = QuaternionEx.Transform(worldConstrainedAxis2, QuaternionEx.Conjugate(ConnectionA.Orientation));
}
///<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 System.Numerics.Vector3 direction, out System.Numerics.Vector3 extremePoint)
{
//Is it the tip of the cone?
float sinThetaSquared = radius * radius / (radius * radius + height * height);
//If d.Y * d.Y / d.LengthSquared >= sinthetaSquared
if (direction.Y > 0 && direction.Y * direction.Y >= direction.LengthSquared() * sinThetaSquared)
{
extremePoint = new System.Numerics.Vector3(0, .75f * height, 0);
return;
}
//Is it a bottom edge of the cone?
float horizontalLengthSquared = direction.X * direction.X + direction.Z * direction.Z;
if (horizontalLengthSquared > Toolbox.Epsilon)
{
var radOverSigma = radius / Math.Sqrt(horizontalLengthSquared);
extremePoint = new System.Numerics.Vector3((float)(radOverSigma * direction.X), -.25f * height, (float)(radOverSigma * direction.Z));
}
else // It's pointing almost straight down...
{
extremePoint = new System.Numerics.Vector3(0, -.25f * height, 0);
}
}
/// <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)
{
//Transform the axes into world space.
Matrix3x3.Transform(ref localHingeAxis, ref connectionA.orientationMatrix, out worldHingeAxis);
Matrix3x3.Transform(ref localTwistAxis, ref connectionB.orientationMatrix, out worldTwistAxis);
//****** VELOCITY BIAS ******//
Vector3Ex.Dot(ref worldHingeAxis, ref worldTwistAxis, out error);
//Compute the correction velocity.
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);
biasVelocity = MathHelper.Clamp(error * errorReduction, -maxCorrectiveVelocity, maxCorrectiveVelocity);
//Compute the jacobian
Vector3Ex.Cross(ref worldHingeAxis, ref worldTwistAxis, out jacobianA);
float length = jacobianA.LengthSquared();
if (length > Toolbox.Epsilon)
{
Vector3Ex.Divide(ref jacobianA, (float)Math.Sqrt(length), out jacobianA);
}
else
{
jacobianA = new System.Numerics.Vector3();
}
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;
System.Numerics.Vector3 transformedAxis;
if (connectionA.isDynamic)
{
Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3Ex.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);
Vector3Ex.Dot(ref transformedAxis, ref jacobianB, out entryB);
}
else
{
entryB = 0;
}
//Compute the inverse mass matrix
velocityToImpulse = 1 / (softness + entryA + entryB);
}
//This works in the general case where there can be any number of contacts and candidates. Could specialize it as an optimization to single-contact added incremental manifolds.
///<summary>
/// Reduces the contact manifold to a good subset.
///</summary>
///<param name="contacts">Contacts to reduce.</param>
///<param name="contactCandidates">Contact candidates to include in the reduction process.</param>
///<param name="contactsToRemove">Contacts that need to removed to reach the reduced state.</param>
///<param name="toAdd">Contact candidates that should be added to reach the reduced state.</param>
///<exception cref="InvalidOperationException">Thrown when the set being reduced is empty.</exception>
public static void ReduceContacts(RawList <Contact> contacts, ref QuickList <ContactData> contactCandidates, RawList <int> contactsToRemove, ref QuickList <ContactData> toAdd)
{
//Find the deepest point of all contacts/candidates, as well as a compounded 'normal' vector.
float maximumDepth = -float.MaxValue;
int deepestIndex = -1;
System.Numerics.Vector3 normal = Toolbox.ZeroVector;
for (int i = 0; i < contacts.Count; i++)
{
Vector3Ex.Add(ref normal, ref contacts.Elements[i].Normal, out normal);
if (contacts.Elements[i].PenetrationDepth > maximumDepth)
{
deepestIndex = i;
maximumDepth = contacts.Elements[i].PenetrationDepth;
}
}
for (int i = 0; i < contactCandidates.Count; i++)
{
Vector3Ex.Add(ref normal, ref contactCandidates.Elements[i].Normal, out normal);
if (contactCandidates.Elements[i].PenetrationDepth > maximumDepth)
{
deepestIndex = contacts.Count + i;
maximumDepth = contactCandidates.Elements[i].PenetrationDepth;
}
}
//If the normals oppose each other, this can happen. It doesn't need to be normalized, but having SOME normal is necessary.
if (normal.LengthSquared() < Toolbox.Epsilon)
{
if (contacts.Count > 0)
{
normal = contacts.Elements[0].Normal;
}
else if (contactCandidates.Count > 0)
{
normal = contactCandidates.Elements[0].Normal; //This method is only called when there's too many contacts, so if contacts is empty, the candidates must NOT be empty.
}
else //This method should not have been called at all if it gets here.
{
throw new ArgumentException("Cannot reduce an empty contact set.");
}
}
//Find the contact (candidate) that is furthest away from the deepest contact (candidate).
System.Numerics.Vector3 deepestPosition;
if (deepestIndex < contacts.Count)
{
deepestPosition = contacts.Elements[deepestIndex].Position;
}
else
{
deepestPosition = contactCandidates.Elements[deepestIndex - contacts.Count].Position;
}
float distanceSquared;
float furthestDistance = 0;
int furthestIndex = -1;
for (int i = 0; i < contacts.Count; i++)
{
Vector3Ex.DistanceSquared(ref contacts.Elements[i].Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestDistance = distanceSquared;
furthestIndex = i;
}
}
for (int i = 0; i < contactCandidates.Count; i++)
{
Vector3Ex.DistanceSquared(ref contactCandidates.Elements[i].Position, ref deepestPosition, out distanceSquared);
if (distanceSquared > furthestDistance)
{
furthestDistance = distanceSquared;
furthestIndex = contacts.Count + i;
}
}
if (furthestIndex == -1)
{
//Either this method was called when it shouldn't have been, or all contacts and contact candidates are at the same location.
if (contacts.Count > 0)
{
for (int i = 1; i < contacts.Count; i++)
{
contactsToRemove.Add(i);
}
return;
}
if (contactCandidates.Count > 0)
{
toAdd.Add(ref contactCandidates.Elements[0]);
return;
}
throw new ArgumentException("Cannot reduce an empty contact set.");
}
System.Numerics.Vector3 furthestPosition;
if (furthestIndex < contacts.Count)
{
furthestPosition = contacts.Elements[furthestIndex].Position;
}
else
{
furthestPosition = contactCandidates.Elements[furthestIndex - contacts.Count].Position;
}
System.Numerics.Vector3 xAxis;
Vector3Ex.Subtract(ref deepestPosition, ref furthestPosition, out xAxis);
//Create the second axis of the 2d 'coordinate system' of the manifold.
System.Numerics.Vector3 yAxis;
Vector3Ex.Cross(ref xAxis, ref normal, out yAxis);
//Determine the furthest points along the axis.
float minYAxisDot = float.MaxValue, maxYAxisDot = -float.MaxValue;
int minYAxisIndex = -1, maxYAxisIndex = -1;
for (int i = 0; i < contacts.Count; i++)
{
float dot;
Vector3Ex.Dot(ref contacts.Elements[i].Position, ref yAxis, out dot);
if (dot < minYAxisDot)
{
minYAxisIndex = i;
minYAxisDot = dot;
}
if (dot > maxYAxisDot)
{
maxYAxisIndex = i;
maxYAxisDot = dot;
}
}
for (int i = 0; i < contactCandidates.Count; i++)
{
float dot;
Vector3Ex.Dot(ref contactCandidates.Elements[i].Position, ref yAxis, out dot);
if (dot < minYAxisDot)
{
minYAxisIndex = i + contacts.Count;
minYAxisDot = dot;
}
if (dot > maxYAxisDot)
{
maxYAxisIndex = i + contacts.Count;
maxYAxisDot = dot;
}
}
//the deepestIndex, furthestIndex, minYAxisIndex, and maxYAxisIndex are the extremal points.
//Cycle through the existing contacts. If any DO NOT MATCH the existing candidates, add them to the toRemove list.
//Cycle through the candidates. If any match, add them to the toAdd list.
//Repeated entries in the reduced manifold aren't a problem.
//-Contacts list does not include repeats with itself.
//-A contact is only removed if it doesn't match anything.
//-Contact candidates do not repeat with themselves.
//-Contact candidates do not repeat with contacts.
//-Contact candidates are added if they match any of the indices.
for (int i = 0; i < contactCandidates.Count; i++)
{
int totalIndex = i + contacts.Count;
if (totalIndex == deepestIndex || totalIndex == furthestIndex || totalIndex == minYAxisIndex || totalIndex == maxYAxisIndex)
{
//This contact is present in the new manifold. Add it.
toAdd.Add(ref contactCandidates.Elements[i]);
}
}
for (int i = 0; i < contacts.Count; i++)
{
if (!(i == deepestIndex || i == furthestIndex || i == minYAxisIndex || i == maxYAxisIndex))
{
//This contact is not present in the new manifold. Remove it.
contactsToRemove.Add(i);
}
}
}
private void UpdateRestrictedAxes()
{
localConstrainedAxis1 = System.Numerics.Vector3.Cross(Vector3Ex.Up, localAxisA);
if (localConstrainedAxis1.LengthSquared() < .001f)
{
localConstrainedAxis1 = System.Numerics.Vector3.Cross(Vector3Ex.Right, localAxisA);
}
localConstrainedAxis2 = System.Numerics.Vector3.Cross(localAxisA, localConstrainedAxis1);
localConstrainedAxis1.Normalize();
localConstrainedAxis2.Normalize();
}
///<summary>
/// Adds a new point to the simplex.
///</summary>
///<param name="shapeA">First shape in the pair.</param>
///<param name="shapeB">Second shape in the pair.</param>
///<param name="iterationCount">Current iteration count.</param>
///<param name="closestPoint">Current point on simplex closest to origin.</param>
///<returns>Whether or not GJK should exit due to a lack of progression.</returns>
public bool GetNewSimplexPoint(ConvexShape shapeA, ConvexShape shapeB, int iterationCount, ref System.Numerics.Vector3 closestPoint)
{
System.Numerics.Vector3 negativeDirection;
Vector3Ex.Negate(ref closestPoint, out negativeDirection);
System.Numerics.Vector3 sa, sb;
shapeA.GetLocalExtremePointWithoutMargin(ref negativeDirection, out sa);
shapeB.GetExtremePointWithoutMargin(closestPoint, ref LocalTransformB, out sb);
System.Numerics.Vector3 S;
Vector3Ex.Subtract(ref sa, ref sb, out S);
//If S is not further towards the origin along negativeDirection than closestPoint, then we're done.
float dotS;
Vector3Ex.Dot(ref S, ref negativeDirection, out dotS); //-P * S
float distanceToClosest = closestPoint.LengthSquared();
float progression = dotS + distanceToClosest;
//It's likely that the system is oscillating between two or more states, usually because of a degenerate simplex.
//Rather than detect specific problem cases, this approach just lets it run and catches whatever falls through.
//During oscillation, one of the states is usually just BARELY outside of the numerical tolerance.
//After a bunch of iterations, the system lets it pick the 'better' one.
if (iterationCount > GJKToolbox.HighGJKIterations && distanceToClosest - previousDistanceToClosest < DistanceConvergenceEpsilon * errorTolerance)
{
return(true);
}
if (distanceToClosest < previousDistanceToClosest)
{
previousDistanceToClosest = distanceToClosest;
}
//If "A" is the new point always, then the switch statement can be removed
//in favor of just pushing three points up.
switch (State)
{
case SimplexState.Point:
if (progression <= (errorTolerance = MathHelper.Max(A.LengthSquared(), S.LengthSquared())) * ProgressionEpsilon)
{
return(true);
}
State = SimplexState.Segment;
B = S;
SimplexA.B = sa;
SimplexB.B = sb;
return(false);
case SimplexState.Segment:
if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), S.LengthSquared())) * ProgressionEpsilon)
{
return(true);
}
State = SimplexState.Triangle;
C = S;
SimplexA.C = sa;
SimplexB.C = sb;
return(false);
case SimplexState.Triangle:
if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), MathHelper.Max(C.LengthSquared(), S.LengthSquared()))) * ProgressionEpsilon)
{
return(true);
}
State = SimplexState.Tetrahedron;
D = S;
SimplexA.D = sa;
SimplexB.D = sb;
return(false);
}
return(false);
}
///<summary>
/// Casts a fat (sphere expanded) ray against the shape.
///</summary>
///<param name="ray">Ray to test against the shape.</param>
///<param name="radius">Radius of the ray.</param>
///<param name="shape">Shape to test against.</param>
///<param name="shapeTransform">Transform to apply to the shape for the test.</param>
///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
///<param name="hit">Hit data of the sphere cast, if any.</param>
///<returns>Whether or not the sphere cast hit the shape.</returns>
public static bool SphereCast(Ray ray, float radius, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength,
out RayHit hit)
{
//Transform the ray into the object's local space.
Vector3Ex.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
System.Numerics.Quaternion conjugate;
QuaternionEx.Conjugate(ref shapeTransform.Orientation, out conjugate);
QuaternionEx.Transform(ref ray.Position, ref conjugate, out ray.Position);
QuaternionEx.Transform(ref ray.Direction, ref conjugate, out ray.Direction);
System.Numerics.Vector3 w, p;
hit.T = 0;
hit.Location = ray.Position;
hit.Normal = Toolbox.ZeroVector;
System.Numerics.Vector3 v = hit.Location;
RaySimplex simplex = new RaySimplex();
float vw, vdir;
int count = 0;
//This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position))
{
if (++count > MaximumGJKIterations)
{
//It's taken too long to find a hit. Numerical problems are probable; quit.
hit = new RayHit();
return(false);
}
shape.GetLocalExtremePointWithoutMargin(ref v, out p);
System.Numerics.Vector3 contribution;
MinkowskiToolbox.ExpandMinkowskiSum(shape.collisionMargin, radius, ref v, out contribution);
Vector3Ex.Add(ref p, ref contribution, out p);
Vector3Ex.Subtract(ref hit.Location, ref p, out w);
Vector3Ex.Dot(ref v, ref w, out vw);
if (vw > 0)
{
Vector3Ex.Dot(ref v, ref ray.Direction, out vdir);
hit.T = hit.T - vw / vdir;
if (vdir >= 0)
{
//We would have to back up!
return(false);
}
if (hit.T > maximumLength)
{
//If we've gone beyond where the ray can reach, there's obviously no hit.
return(false);
}
//Shift the ray up.
Vector3Ex.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref ray.Position, out hit.Location);
hit.Normal = v;
}
RaySimplex shiftedSimplex;
simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex);
shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v);
}
//Transform the hit data into world space.
QuaternionEx.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
QuaternionEx.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);
return(true);
}
///<summary>
/// Sweeps two shapes against another.
///</summary>
///<param name="shapeA">First shape being swept.</param>
///<param name="shapeB">Second shape being swept.</param>
///<param name="sweepA">Sweep vector for the first shape.</param>
///<param name="sweepB">Sweep vector for the second shape.</param>
///<param name="transformA">Transform to apply to the first shape.</param>
///<param name="transformB">Transform to apply to the second shape.</param>
///<param name="hit">Hit data of the sweep test, if any.</param>
///<returns>Whether or not the swept shapes hit each other..</returns>
public static bool ConvexCast(ConvexShape shapeA, ConvexShape shapeB, ref System.Numerics.Vector3 sweepA, ref System.Numerics.Vector3 sweepB, ref RigidTransform transformA, ref RigidTransform transformB,
out RayHit hit)
{
//Put the velocity into shapeA's local space.
System.Numerics.Vector3 velocityWorld;
Vector3Ex.Subtract(ref sweepB, ref sweepA, out velocityWorld);
System.Numerics.Quaternion conjugateOrientationA;
QuaternionEx.Conjugate(ref transformA.Orientation, out conjugateOrientationA);
System.Numerics.Vector3 rayDirection;
QuaternionEx.Transform(ref velocityWorld, ref conjugateOrientationA, out rayDirection);
//Transform b into a's local space.
RigidTransform localTransformB;
QuaternionEx.Concatenate(ref transformB.Orientation, ref conjugateOrientationA, out localTransformB.Orientation);
Vector3Ex.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position);
QuaternionEx.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position);
System.Numerics.Vector3 w, p;
hit.T = 0;
hit.Location = System.Numerics.Vector3.Zero; //The ray starts at the origin.
hit.Normal = Toolbox.ZeroVector;
System.Numerics.Vector3 v = hit.Location;
RaySimplex simplex = new RaySimplex();
float vw, vdir;
int count = 0;
do
{
if (++count > MaximumGJKIterations)
{
//It's taken too long to find a hit. Numerical problems are probable; quit.
hit = new RayHit();
return(false);
}
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref v, ref localTransformB, out p);
Vector3Ex.Subtract(ref hit.Location, ref p, out w);
Vector3Ex.Dot(ref v, ref w, out vw);
if (vw > 0)
{
Vector3Ex.Dot(ref v, ref rayDirection, out vdir);
if (vdir >= 0)
{
hit = new RayHit();
return(false);
}
hit.T = hit.T - vw / vdir;
if (hit.T > 1)
{
//If we've gone beyond where the ray can reach, there's obviously no hit.
hit = new RayHit();
return(false);
}
//Shift the ray up.
Vector3Ex.Multiply(ref rayDirection, hit.T, out hit.Location);
//The ray origin is the origin! Don't need to add any ray position.
hit.Normal = v;
}
RaySimplex shiftedSimplex;
simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex);
shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v);
//Could measure the progress of the ray. If it's too little, could early out.
//Not used by default since it's biased towards precision over performance.
} while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref Toolbox.ZeroVector));
//This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
//Transform the hit data into world space.
QuaternionEx.Transform(ref hit.Normal, ref transformA.Orientation, out hit.Normal);
Vector3Ex.Multiply(ref velocityWorld, hit.T, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref transformA.Position, out hit.Location);
return(true);
}
//TODO: Consider changing the termination epsilons on these casts. Epsilon * Modifier is okay, but there might be better options.
///<summary>
/// Tests a ray against a convex shape.
///</summary>
///<param name="ray">Ray to test against the shape.</param>
///<param name="shape">Shape to test.</param>
///<param name="shapeTransform">Transform to apply to the shape for the test.</param>
///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
///<param name="hit">Hit data of the ray cast, if any.</param>
///<returns>Whether or not the ray hit the shape.</returns>
public static bool RayCast(Ray ray, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength,
out RayHit hit)
{
//Transform the ray into the object's local space.
Vector3Ex.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
System.Numerics.Quaternion conjugate;
QuaternionEx.Conjugate(ref shapeTransform.Orientation, out conjugate);
QuaternionEx.Transform(ref ray.Position, ref conjugate, out ray.Position);
QuaternionEx.Transform(ref ray.Direction, ref conjugate, out ray.Direction);
System.Numerics.Vector3 extremePointToRayOrigin, extremePoint;
hit.T = 0;
hit.Location = ray.Position;
hit.Normal = Toolbox.ZeroVector;
System.Numerics.Vector3 closestOffset = hit.Location;
RaySimplex simplex = new RaySimplex();
float vw, closestPointDotDirection;
int count = 0;
//This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
while (closestOffset.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position))
{
if (++count > MaximumGJKIterations)
{
//It's taken too long to find a hit. Numerical problems are probable; quit.
hit = new RayHit();
return(false);
}
shape.GetLocalExtremePoint(closestOffset, out extremePoint);
Vector3Ex.Subtract(ref hit.Location, ref extremePoint, out extremePointToRayOrigin);
Vector3Ex.Dot(ref closestOffset, ref extremePointToRayOrigin, out vw);
//If the closest offset and the extreme point->ray origin direction point the same way,
//then we might be able to conservatively advance the point towards the surface.
if (vw > 0)
{
Vector3Ex.Dot(ref closestOffset, ref ray.Direction, out closestPointDotDirection);
if (closestPointDotDirection >= 0)
{
hit = new RayHit();
return(false);
}
hit.T = hit.T - vw / closestPointDotDirection;
if (hit.T > maximumLength)
{
//If we've gone beyond where the ray can reach, there's obviously no hit.
hit = new RayHit();
return(false);
}
//Shift the ray up.
Vector3Ex.Multiply(ref ray.Direction, hit.T, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref ray.Position, out hit.Location);
hit.Normal = closestOffset;
}
RaySimplex shiftedSimplex;
simplex.AddNewSimplexPoint(ref extremePoint, ref hit.Location, out shiftedSimplex);
//Compute the offset from the simplex surface to the origin.
shiftedSimplex.GetPointClosestToOrigin(ref simplex, out closestOffset);
}
//Transform the hit data into world space.
QuaternionEx.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
QuaternionEx.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);
return(true);
}
///<summary>
/// Adds a new point to the simplex.
///</summary>
///<param name="shapeA">First shape in the pair.</param>
///<param name="shapeB">Second shape in the pair.</param>
///<param name="iterationCount">Current iteration count.</param>
///<param name="closestPoint">Current point on simplex closest to origin.</param>
///<returns>Whether or not GJK should exit due to a lack of progression.</returns>
public bool GetNewSimplexPoint(ConvexShape shapeA, ConvexShape shapeB, int iterationCount, ref System.Numerics.Vector3 closestPoint)
{
System.Numerics.Vector3 negativeDirection;
Vector3Ex.Negate(ref closestPoint, out negativeDirection);
System.Numerics.Vector3 sa, sb;
shapeA.GetLocalExtremePointWithoutMargin(ref negativeDirection, out sa);
shapeB.GetExtremePointWithoutMargin(closestPoint, ref LocalTransformB, out sb);
System.Numerics.Vector3 S;
Vector3Ex.Subtract(ref sa, ref sb, out S);
//If S is not further towards the origin along negativeDirection than closestPoint, then we're done.
float dotS;
Vector3Ex.Dot(ref S, ref negativeDirection, out dotS); //-P * S
float distanceToClosest = closestPoint.LengthSquared();
float progression = dotS + distanceToClosest;
//It's likely that the system is oscillating between two or more states, usually because of a degenerate simplex.
//Rather than detect specific problem cases, this approach just lets it run and catches whatever falls through.
//During oscillation, one of the states is usually just BARELY outside of the numerical tolerance.
//After a bunch of iterations, the system lets it pick the 'better' one.
if (iterationCount > GJKToolbox.HighGJKIterations && distanceToClosest - previousDistanceToClosest < DistanceConvergenceEpsilon * errorTolerance)
return true;
if (distanceToClosest < previousDistanceToClosest)
previousDistanceToClosest = distanceToClosest;
//If "A" is the new point always, then the switch statement can be removed
//in favor of just pushing three points up.
switch (State)
{
case SimplexState.Point:
if (progression <= (errorTolerance = MathHelper.Max(A.LengthSquared(), S.LengthSquared())) * ProgressionEpsilon)
return true;
State = SimplexState.Segment;
B = S;
SimplexA.B = sa;
SimplexB.B = sb;
return false;
case SimplexState.Segment:
if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), S.LengthSquared())) * ProgressionEpsilon)
return true;
State = SimplexState.Triangle;
C = S;
SimplexA.C = sa;
SimplexB.C = sb;
return false;
case SimplexState.Triangle:
if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), MathHelper.Max(C.LengthSquared(), S.LengthSquared()))) * ProgressionEpsilon)
return true;
State = SimplexState.Tetrahedron;
D = S;
SimplexA.D = sa;
SimplexB.D = sb;
return false;
}
return false;
}
internal void PreStep(float dt)
{
vehicleEntity = wheel.Vehicle.Body;
supportEntity = wheel.SupportingEntity;
supportIsDynamic = supportEntity != null && supportEntity.isDynamic;
Vector3Ex.Cross(ref wheel.worldForwardDirection, ref wheel.normal, out slidingFrictionAxis);
float axisLength = slidingFrictionAxis.LengthSquared();
//Safety against bad cross product
if (axisLength < Toolbox.BigEpsilon)
{
Vector3Ex.Cross(ref wheel.worldForwardDirection, ref Toolbox.UpVector, out slidingFrictionAxis);
axisLength = slidingFrictionAxis.LengthSquared();
if (axisLength < Toolbox.BigEpsilon)
{
Vector3Ex.Cross(ref wheel.worldForwardDirection, ref Toolbox.RightVector, out slidingFrictionAxis);
}
}
slidingFrictionAxis.Normalize();
linearAX = slidingFrictionAxis.X;
linearAY = slidingFrictionAxis.Y;
linearAZ = slidingFrictionAxis.Z;
//angular A = Ra x N
angularAX = (wheel.ra.Y * linearAZ) - (wheel.ra.Z * linearAY);
angularAY = (wheel.ra.Z * linearAX) - (wheel.ra.X * linearAZ);
angularAZ = (wheel.ra.X * linearAY) - (wheel.ra.Y * linearAX);
//Angular B = N x Rb
angularBX = (linearAY * wheel.rb.Z) - (linearAZ * wheel.rb.Y);
angularBY = (linearAZ * wheel.rb.X) - (linearAX * wheel.rb.Z);
angularBZ = (linearAX * wheel.rb.Y) - (linearAY * wheel.rb.X);
//Compute inverse effective mass matrix
float entryA, entryB;
//these are the transformed coordinates
float tX, tY, tZ;
if (vehicleEntity.isDynamic)
{
tX = angularAX * vehicleEntity.inertiaTensorInverse.M11 + angularAY * vehicleEntity.inertiaTensorInverse.M21 + angularAZ * vehicleEntity.inertiaTensorInverse.M31;
tY = angularAX * vehicleEntity.inertiaTensorInverse.M12 + angularAY * vehicleEntity.inertiaTensorInverse.M22 + angularAZ * vehicleEntity.inertiaTensorInverse.M32;
tZ = angularAX * vehicleEntity.inertiaTensorInverse.M13 + angularAY * vehicleEntity.inertiaTensorInverse.M23 + angularAZ * vehicleEntity.inertiaTensorInverse.M33;
entryA = tX * angularAX + tY * angularAY + tZ * angularAZ + vehicleEntity.inverseMass;
}
else
{
entryA = 0;
}
if (supportIsDynamic)
{
tX = angularBX * supportEntity.inertiaTensorInverse.M11 + angularBY * supportEntity.inertiaTensorInverse.M21 + angularBZ * supportEntity.inertiaTensorInverse.M31;
tY = angularBX * supportEntity.inertiaTensorInverse.M12 + angularBY * supportEntity.inertiaTensorInverse.M22 + angularBZ * supportEntity.inertiaTensorInverse.M32;
tZ = angularBX * supportEntity.inertiaTensorInverse.M13 + angularBY * supportEntity.inertiaTensorInverse.M23 + angularBZ * supportEntity.inertiaTensorInverse.M33;
entryB = tX * angularBX + tY * angularBY + tZ * angularBZ + supportEntity.inverseMass;
}
else
{
entryB = 0;
}
velocityToImpulse = -1 / (entryA + entryB); //Softness?
//Compute friction.
//Which coefficient? Check velocity.
if (Math.Abs(RelativeVelocity) < staticFrictionVelocityThreshold)
{
blendedCoefficient = frictionBlender(staticCoefficient, wheel.supportMaterial.staticFriction, false, wheel);
}
else
{
blendedCoefficient = frictionBlender(kineticCoefficient, wheel.supportMaterial.kineticFriction, true, wheel);
}
}
/// <summary>
/// Updates the collection of supporting contacts.
/// </summary>
public void UpdateSupports(ref System.Numerics.Vector3 movementDirection)
{
bool hadTraction = HasTraction;
//Reset traction/support.
HasTraction = false;
HasSupport = false;
System.Numerics.Vector3 downDirection = characterBody.orientationMatrix.Down;
System.Numerics.Vector3 bodyPosition = characterBody.position;
//Compute the character's radius, minus a little margin. We want the rays to originate safely within the character's body.
//Assume vertical rotational invariance. Spheres, cylinders, and capsules don't have varying horizontal radii.
System.Numerics.Vector3 extremePoint;
var convexShape = characterBody.CollisionInformation.Shape as ConvexShape;
Debug.Assert(convexShape != null, "Character bodies must be convex.");
//Find the lowest point on the collision shape.
convexShape.GetLocalExtremePointWithoutMargin(ref Toolbox.DownVector, out extremePoint);
BottomDistance = -extremePoint.Y + convexShape.collisionMargin;
convexShape.GetLocalExtremePointWithoutMargin(ref Toolbox.RightVector, out extremePoint);
float rayCastInnerRadius = Math.Max((extremePoint.X + convexShape.collisionMargin) * 0.8f, extremePoint.X);
//Vertically, the rays will start at the same height as the character's center.
//While they could be started lower on a cylinder, that wouldn't always work for a sphere or capsule: the origin might end up outside of the shape!
tractionContacts.Clear();
supportContacts.Clear();
sideContacts.Clear();
headContacts.Clear();
foreach (var pair in characterBody.CollisionInformation.Pairs)
{
//Don't stand on things that aren't really colliding fully.
if (pair.CollisionRule != CollisionRule.Normal)
{
continue;
}
ContactCategorizer.CategorizeContacts(pair, characterBody.CollisionInformation, ref downDirection, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts);
}
HasSupport = supportContacts.Count > 0;
HasTraction = tractionContacts.Count > 0;
//Only perform ray casts if the character has fully left the surface, and only if the previous frame had traction.
//(If ray casts are allowed when support contacts still exist, the door is opened for climbing surfaces which should not be climbable.
//Consider a steep slope. If the character runs at it, the character will likely be wedged off of the ground, making it lose traction while still having a support contact with the slope.
//If ray tests are allowed when support contacts exist, the character will maintain traction despite climbing the wall.
//The VerticalMotionConstraint can stop the character from climbing in many cases, but it's nice not to have to rely on it.
//Disallowing ray tests when supports exist does have a cost, though. For example, consider rounded steps.
//If the character walks off a step such that it is still in contact with the step but is far enough down that the slope is too steep for traction,
//the ray test won't recover traction. This situation just isn't very common.)
if (!HasSupport && hadTraction)
{
float supportRayLength = maximumAssistedDownStepHeight + BottomDistance;
SupportRayData = null;
//If the contacts aren't available to support the character, raycast down to find the ground.
if (!HasTraction)
{
//TODO: could also require that the character has a nonzero movement direction in order to use a ray cast. Questionable- would complicate the behavior on edges.
Ray ray = new Ray(bodyPosition, downDirection);
bool hasTraction;
SupportRayData data;
if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data))
{
SupportRayData = data;
HasTraction = data.HasTraction;
HasSupport = true;
}
}
//If contacts and the center ray cast failed, try a ray offset in the movement direction.
bool tryingToMove = movementDirection.LengthSquared() > 0;
if (!HasTraction && tryingToMove)
{
Ray ray = new Ray(
characterBody.Position +
movementDirection * rayCastInnerRadius, downDirection);
//Have to test to make sure the ray doesn't get obstructed. This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support!
Ray obstructionRay;
obstructionRay.Position = characterBody.Position;
obstructionRay.Direction = ray.Position - obstructionRay.Position;
if (!QueryManager.RayCastHitAnything(obstructionRay, 1))
{
//The origin isn't obstructed, so now ray cast down.
bool hasTraction;
SupportRayData data;
if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data))
{
if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T)
{
//Only replace the previous support ray if we now have traction or we didn't have a support ray at all before,
//or this hit is a better (sooner) hit.
if (hasTraction)
{
SupportRayData = data;
HasTraction = true;
}
else if (SupportRayData == null)
{
SupportRayData = data;
}
HasSupport = true;
}
}
}
}
//If contacts, center ray, AND forward ray failed to find traction, try a side ray created from down x forward.
if (!HasTraction && tryingToMove)
{
//Compute the horizontal offset direction. Down direction and the movement direction are normalized and perpendicular, so the result is too.
System.Numerics.Vector3 horizontalOffset;
Vector3Ex.Cross(ref movementDirection, ref downDirection, out horizontalOffset);
Vector3Ex.Multiply(ref horizontalOffset, rayCastInnerRadius, out horizontalOffset);
Ray ray = new Ray(bodyPosition + horizontalOffset, downDirection);
//Have to test to make sure the ray doesn't get obstructed. This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support!
Ray obstructionRay;
obstructionRay.Position = bodyPosition;
obstructionRay.Direction = ray.Position - obstructionRay.Position;
if (!QueryManager.RayCastHitAnything(obstructionRay, 1))
{
//The origin isn't obstructed, so now ray cast down.
bool hasTraction;
SupportRayData data;
if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data))
{
if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T)
{
//Only replace the previous support ray if we now have traction or we didn't have a support ray at all before,
//or this hit is a better (sooner) hit.
if (hasTraction)
{
SupportRayData = data;
HasTraction = true;
}
else if (SupportRayData == null)
{
SupportRayData = data;
}
HasSupport = true;
}
}
}
}
//If contacts, center ray, forward ray, AND the first side ray failed to find traction, try a side ray created from forward x down.
if (!HasTraction && tryingToMove)
{
//Compute the horizontal offset direction. Down direction and the movement direction are normalized and perpendicular, so the result is too.
System.Numerics.Vector3 horizontalOffset;
Vector3Ex.Cross(ref downDirection, ref movementDirection, out horizontalOffset);
Vector3Ex.Multiply(ref horizontalOffset, rayCastInnerRadius, out horizontalOffset);
Ray ray = new Ray(bodyPosition + horizontalOffset, downDirection);
//Have to test to make sure the ray doesn't get obstructed. This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support!
Ray obstructionRay;
obstructionRay.Position = bodyPosition;
obstructionRay.Direction = ray.Position - obstructionRay.Position;
if (!QueryManager.RayCastHitAnything(obstructionRay, 1))
{
//The origin isn't obstructed, so now ray cast down.
bool hasTraction;
SupportRayData data;
if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data))
{
if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T)
{
//Only replace the previous support ray if we now have traction or we didn't have a support ray at all before,
//or this hit is a better (sooner) hit.
if (hasTraction)
{
SupportRayData = data;
HasTraction = true;
}
else if (SupportRayData == null)
{
SupportRayData = data;
}
HasSupport = true;
}
}
}
}
}
UpdateSupportData(ref downDirection);
UpdateVerticalSupportData(ref downDirection, ref movementDirection);
}
/// <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 System.Numerics.Matrix4x4
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);
Vector3Ex.Add(ref connectionB.position, ref worldOffsetB, out error);
Vector3Ex.Subtract(ref error, ref connectionA.position, out error);
Vector3Ex.Subtract(ref error, ref worldOffsetA, out error);
Vector3Ex.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>
/// Initializes the constraint for the current frame.
/// </summary>
/// <param name="dt">Time between frames.</param>
public override void Update(float dt)
{
basis.rotationMatrix = connectionA.orientationMatrix;
basis.ComputeWorldSpaceAxes();
if (settings.mode == MotorMode.Servomechanism) //Only need to do the bulk of this work if it's a servo.
{
//The error is computed using this equation:
//GoalRelativeOrientation * ConnectionA.Orientation * Error = ConnectionB.Orientation
//GoalRelativeOrientation is the original rotation from A to B in A's local space.
//Multiplying by A's orientation gives us where B *should* be.
//Of course, B won't be exactly where it should be after initialization.
//The Error component holds the difference between what is and what should be.
//Error = (GoalRelativeOrientation * ConnectionA.Orientation)^-1 * ConnectionB.Orientation
//ConnectionA.Orientation is replaced in the above by the world space basis orientation.
System.Numerics.Quaternion worldBasis = QuaternionEx.CreateFromRotationMatrix(basis.WorldTransform);
System.Numerics.Quaternion bTarget;
QuaternionEx.Concatenate(ref settings.servo.goal, ref worldBasis, out bTarget);
System.Numerics.Quaternion bTargetConjugate;
QuaternionEx.Conjugate(ref bTarget, out bTargetConjugate);
System.Numerics.Quaternion error;
QuaternionEx.Concatenate(ref bTargetConjugate, ref connectionB.orientation, out error);
float errorReduction;
settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out usedSoftness);
//Turn this into an axis-angle representation.
QuaternionEx.GetAxisAngleFromQuaternion(ref error, out axis, out angle);
//Scale the axis by the desired velocity if the angle is sufficiently large (epsilon).
if (angle > Toolbox.BigEpsilon)
{
float velocity = -(MathHelper.Min(settings.servo.baseCorrectiveSpeed, angle / dt) + angle * errorReduction);
biasVelocity.X = axis.X * velocity;
biasVelocity.Y = axis.Y * velocity;
biasVelocity.Z = axis.Z * velocity;
//Ensure that the corrective velocity doesn't exceed the max.
float length = biasVelocity.LengthSquared();
if (length > settings.servo.maxCorrectiveVelocitySquared)
{
float multiplier = settings.servo.maxCorrectiveVelocity / (float)Math.Sqrt(length);
biasVelocity.X *= multiplier;
biasVelocity.Y *= multiplier;
biasVelocity.Z *= multiplier;
}
}
else
{
biasVelocity.X = 0;
biasVelocity.Y = 0;
biasVelocity.Z = 0;
}
}
else
{
usedSoftness = settings.velocityMotor.softness / dt;
angle = 0; //Zero out the error;
Matrix3x3 transform = basis.WorldTransform;
Matrix3x3.Transform(ref settings.velocityMotor.goalVelocity, ref transform, out biasVelocity);
}
//Compute effective mass
Matrix3x3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out effectiveMassMatrix);
effectiveMassMatrix.M11 += usedSoftness;
effectiveMassMatrix.M22 += usedSoftness;
effectiveMassMatrix.M33 += usedSoftness;
Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
//Update the maximum force
ComputeMaxForces(settings.maximumForce, dt);
}
/// <summary>
/// 相交点辐射光
/// </summary>
/// <param name="point"></param>
/// <param name="normal"></param>
/// <param name="deep"></param>
/// <returns></returns>
public virtual Light IntersectLight(Vector3 point, Vector3 dir, Vector3 normal, int deep)
{
#if RayDebugger
SceneDebug Debugger = Scene.debugger;
if (Debugger != null)
{
Debugger.BeginBranch(point);
}
#endif
Light returnlight = default;
// 发光体返回发光颜色
if (Material.LightAble)
{
returnlight = Material.LightColor;
goto returnPoint;
}
// 递归深度极限
if (deep <= 1)
{
returnlight = Material.BaseColor * 0.3f;
goto returnPoint;
}
dir = Vector3.Normalize(dir);
bool IsBackFace = false;
if (Vector3.Dot(dir, normal) > 0) // 背面
{
IsBackFace = true;
normal = -normal;
}
#region 计算追踪光线总数
int traceRayNum = RenderConfiguration.Configurations.ReflectSmapingLevel - RenderConfiguration.Configurations.RayTraceDeep + deep;
{
traceRayNum = (int)(traceRayNum * Material.AMetalDegree);
if (traceRayNum < 1)
{
traceRayNum = 1;
}
traceRayNum = traceRayNum * 3 - 2;
}
#endregion
#region 计算折射光
Light refractl = default; // 折射光
float refractPower = 0.0f; // 折射光强度
if (Material.IsTransparent)
{
float riindex = Material.RefractiveIndices;
if (!IsBackFace)
{
riindex = 1.0f / riindex;
}
// 计算折射光线
(float pow, Vector3 rdir) = Tools.Refract(dir, normal, riindex);
if (pow < 0)
{
goto endRefract;
}
refractPower = pow * Material.TransparentIndex;
int raycount = (int)(traceRayNum * refractPower);
traceRayNum -= raycount;
float randomScale = Material.AMetalDegree * Material.AMetalDegree * 0.5f;
//raycount = (int)(traceRayNum * randomScale);
//raycount = (int)(traceRayNum * Material.AMetalDegree);
if (raycount < 1 && refractPower > 0.00001)
{
raycount = 1;
}
if (raycount == 0)
{
refractl = Material.BaseColor;
goto endRefract;
}
rdir = normal * randomScale + rdir * (1.0f - randomScale);
for (int nsmap = 0; nsmap < raycount; nsmap++)
{
Vector3 raydir = Tools.RandomPointInSphere() * randomScale + rdir;
Ray r = new Ray(point, raydir);
//Console.WriteLine('\t' + this.Name + " [refract] : " + r);
(Light c, float distance) = Scene.Light(r, deep - 1, this);
if (IsBackFace) //内部光线，进行吸收计算
{
float xsl = Math.Log(distance + 1.0f) + 1.0f;
refractl *= Material.BaseColor / xsl;
}
refractl += c;
}
refractl /= raycount;
}
endRefract:
#endregion
#region 计算反射光
Light reflectl = default; // 反射光
{
int raycount = traceRayNum;
if (raycount < 1)
{
if (refractPower < 0.99f)
{
raycount = 1;
}
else
{
reflectl = Material.BaseColor;
goto endReflact;
}
}
Vector3 spO;
{
//Vector3 spRO = Tools.Reflect(dir, normal);
Vector3 spRO = Vector3.Reflect(dir, normal);
//spO = normal * (1.0f - Material.MetalDegree) + spRO * Material.MetalDegree;
spO = Vector3.Lerp(normal, spRO, Material.MetalDegree);
}
for (int nsmap = 0; nsmap < raycount; nsmap++)
{
Vector3 tp = Tools.RandomPointInSphere() * Material.AMetalDegree + spO;
Vector3 raydir = tp;
while (raydir.LengthSquared() < 0.1)
{
tp = Tools.RandomPointInSphere() + spO;
raydir = tp;
}
Ray r = new Ray(point, raydir);
//Console.WriteLine('\t' + this.Name + " [reflact] : " + r);
(Light c, float _) = Scene.Light(r, deep - 1, this); //, this);
reflectl += c;
}
reflectl /= raycount;
reflectl *= (0.06f * Material.MetalDegree + 0.93f) * Material.BaseColor;
}
#endregion
returnlight = refractl * refractPower + reflectl * (1.0f - refractPower);
endReflact:
returnPoint:
#if RayDebugger
if (Debugger != null)
{
Debugger.EndBranch();
}
#endif
return(returnlight);
}
/// <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
System.Numerics.Vector3 lambda;
Vector3Ex.Cross(ref r, ref entity.angularVelocity, out lambda);
Vector3Ex.Subtract(ref lambda, ref entity.linearVelocity, out lambda);
//Add in bias velocity
Vector3Ex.Add(ref biasVelocity, ref lambda, out lambda);
//Add in softness
System.Numerics.Vector3 softnessVelocity;
Vector3Ex.Multiply(ref accumulatedImpulse, usedSoftness, out softnessVelocity);
Vector3Ex.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.
System.Numerics.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);
System.Numerics.Vector3 taImpulse;
Vector3Ex.Cross(ref r, ref lambda, out taImpulse);
entity.ApplyAngularImpulse(ref taImpulse);
return (Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z));
}
