/// <summary>
/// Applies the corrective impulses required by the constraint.
/// </summary>
public override Fix64 SolveIteration()
{
Vector3 velocityDifference;
Vector3.Subtract(ref connectionB.angularVelocity, ref connectionA.angularVelocity, out velocityDifference);
Vector3 softnessVector;
Vector3.Multiply(ref accumulatedImpulse, softness, out softnessVector);
Vector3 lambda;
Vector3.Add(ref velocityDifference, ref biasVelocity, out lambda);
Vector3.Subtract(ref lambda, ref softnessVector, out lambda);
Matrix3x3.Transform(ref lambda, ref effectiveMassMatrix, out lambda);
Vector3.Add(ref lambda, ref accumulatedImpulse, out accumulatedImpulse);
if (connectionA.isDynamic)
{
connectionA.ApplyAngularImpulse(ref lambda);
}
if (connectionB.isDynamic)
{
Vector3 torqueB;
Vector3.Negate(ref lambda, out torqueB);
connectionB.ApplyAngularImpulse(ref torqueB);
}
return(Fix64.Abs(lambda.X) + Fix64.Abs(lambda.Y) + Fix64.Abs(lambda.Z));
}
private bool IsContactUnique(ref ContactData contactCandidate, ref QuickList <ContactData> candidatesToAdd)
{
contactCandidate.Validate();
Fix64 distanceSquared;
RigidTransform meshTransform = MeshTransform;
for (int i = 0; i < contacts.Count; i++)
{
Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref contactCandidate.Position, out distanceSquared);
if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
{
//This is a nonconvex manifold. There will be times where a an object will be shoved into a corner such that
//a single position will have two reasonable normals. If the normals aren't mostly aligned, they should NOT be considered equivalent.
Vector3.Dot(ref contacts.Elements[i].Normal, ref contactCandidate.Normal, out distanceSquared);
if (Fix64.Abs(distanceSquared) >= CollisionDetectionSettings.nonconvexNormalDotMinimum)
{
//Update the existing 'redundant' contact with the new information.
//This works out because the new contact is the deepest contact according to the previous collision detection iteration.
contacts.Elements[i].Normal = contactCandidate.Normal;
contacts.Elements[i].Position = contactCandidate.Position;
contacts.Elements[i].PenetrationDepth = contactCandidate.PenetrationDepth;
supplementData.Elements[i].BasePenetrationDepth = contactCandidate.PenetrationDepth;
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref convex.worldTransform, out supplementData.Elements[i].LocalOffsetA);
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref meshTransform, out supplementData.Elements[i].LocalOffsetB);
return(false);
}
}
}
for (int i = 0; i < candidatesToAdd.Count; i++)
{
Vector3.DistanceSquared(ref candidatesToAdd.Elements[i].Position, ref contactCandidate.Position, out distanceSquared);
if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
{
//This is a nonconvex manifold. There will be times where a an object will be shoved into a corner such that
//a single position will have two reasonable normals. If the normals aren't mostly aligned, they should NOT be considered equivalent.
Vector3.Dot(ref candidatesToAdd.Elements[i].Normal, ref contactCandidate.Normal, out distanceSquared);
if (Fix64.Abs(distanceSquared) >= CollisionDetectionSettings.nonconvexNormalDotMinimum)
{
return(false);
}
}
}
//for (int i = 0; i < edgeContacts.count; i++)
//{
// Vector3.DistanceSquared(ref edgeContacts.Elements[i].ContactData.Position, ref contactCandidate.Position, out distanceSquared);
// if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
// {
// return false;
// }
//}
//for (int i = 0; i < vertexContacts.count; i++)
//{
// Vector3.DistanceSquared(ref vertexContacts.Elements[i].ContactData.Position, ref contactCandidate.Position, out distanceSquared);
// if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
// {
// return false;
// }
//}
return(true);
}
public GGame.Math.Fix64 Area()
{
GGame.Math.Fix64 b = Points[0].X - Points[1].X;
GGame.Math.Fix64 h = Points[2].Y - Points[1].Y;
return(Fix64.Abs((b * h * 0.5f)));
}
/// <summary>
/// Computes one iteration of the constraint to meet the solver updateable's goal.
/// </summary>
/// <returns>The rough applied impulse magnitude.</returns>
public override Fix64 SolveIteration()
{
Fix64 velocityA, velocityB;
//Find the velocity contribution from each connection
Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA);
Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB);
//Add in the constraint space bias velocity
Fix64 lambda = -(velocityA + velocityB) - biasVelocity - usedSoftness * accumulatedImpulse;
//Transform to an impulse
lambda *= velocityToImpulse;
//Accumulate the impulse
Fix64 previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = MathHelper.Clamp(accumulatedImpulse + lambda, -maxForceDt, maxForceDt);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
//Apply the impulse
Vector3 impulse;
if (connectionA.isDynamic)
{
Vector3.Multiply(ref jacobianA, lambda, out impulse);
connectionA.ApplyAngularImpulse(ref impulse);
}
if (connectionB.isDynamic)
{
Vector3.Multiply(ref jacobianB, lambda, out impulse);
connectionB.ApplyAngularImpulse(ref impulse);
}
return(Fix64.Abs(lambda));
}
/**
* <summary>Solves a two-dimensional linear program subject to linear
* constraints defined by lines and a circular constraint.</summary>
*
* <param name="lines">Lines defining the linear constraints.</param>
* <param name="numObstLines">Count of obstacle lines.</param>
* <param name="beginLine">The line on which the 2-d linear program
* failed.</param>
* <param name="radius">The radius of the circular constraint.</param>
* <param name="result">A reference to the result of the linear program.
* </param>
*/
private void linearProgram3(IList <Line> lines, int numObstLines, int beginLine, Fix64 radius, ref Vector2 result)
{
Fix64 distance = Fix64.Zero;
for (int i = beginLine; i < lines.Count; ++i)
{
if (RVOMath.det(lines[i].direction, lines[i].point - result) > distance)
{
/* Result does not satisfy constraint of line i. */
IList <Line> projLines = new List <Line>();
for (int ii = 0; ii < numObstLines; ++ii)
{
projLines.Add(lines[ii]);
}
for (int j = numObstLines; j < i; ++j)
{
Line line;
Fix64 determinant = RVOMath.det(lines[i].direction, lines[j].direction);
if (Fix64.Abs(determinant) <= RVOMath.RVO_EPSILON)
{
/* Line i and line j are parallel. */
if (Vector2.Dot(lines[i].direction, lines[j].direction) > Fix64.Zero)
{
/* Line i and line j point in the same direction. */
continue;
}
else
{
/* Line i and line j point in opposite direction. */
line.point = 0.5m * (lines[i].point + lines[j].point);
}
}
else
{
line.point = lines[i].point + (RVOMath.det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction;
}
line.direction = Vector2.Normalize(lines[j].direction - lines[i].direction);
projLines.Add(line);
}
Vector2 tempResult = result;
if (linearProgram2(projLines, radius, new Vector2(-lines[i].direction.Y, lines[i].direction.X), true, ref result) < projLines.Count)
{
/*
* This should in principle not happen. The result is by
* definition already in the feasible region of this
* linear program. If it fails, it is due to small
* Fix64ing point error, and the current result is kept.
*/
result = tempResult;
}
distance = RVOMath.det(lines[i].direction, lines[i].point - result);
}
}
}
/// <summary>
/// Updates the spin velocity and spin angle for the shape.
/// </summary>
/// <param name="dt">Simulation timestep.</param>
internal void UpdateSpin(Fix64 dt)
{
if (wheel.HasSupport && !(wheel.brake.IsBraking && FreezeWheelsWhileBraking))
{
//On the ground, not braking.
spinVelocity = wheel.drivingMotor.RelativeVelocity / Radius;
}
else if (wheel.HasSupport && wheel.brake.IsBraking && FreezeWheelsWhileBraking)
{
//On the ground, braking
Fix64 deceleratedValue = F64.C0;
if (spinVelocity > F64.C0)
{
deceleratedValue = MathHelper.Max(spinVelocity - brakeFreezeWheelDeceleration * dt, F64.C0);
}
else if (spinVelocity < F64.C0)
{
deceleratedValue = MathHelper.Min(spinVelocity + brakeFreezeWheelDeceleration * dt, F64.C0);
}
spinVelocity = wheel.drivingMotor.RelativeVelocity / Radius;
if (Fix64.Abs(deceleratedValue) < Fix64.Abs(spinVelocity))
{
spinVelocity = deceleratedValue;
}
}
else if (!wheel.HasSupport && wheel.drivingMotor.TargetSpeed != F64.C0)
{
//Airborne and accelerating, increase spin velocity.
Fix64 maxSpeed = Fix64.Abs(wheel.drivingMotor.TargetSpeed) / Radius;
spinVelocity = MathHelper.Clamp(spinVelocity + Fix64.Sign(wheel.drivingMotor.TargetSpeed) * airborneWheelAcceleration * dt, -maxSpeed, maxSpeed);
}
else if (!wheel.HasSupport && wheel.Brake.IsBraking)
{
//Airborne and braking
if (spinVelocity > F64.C0)
{
spinVelocity = MathHelper.Max(spinVelocity - brakeFreezeWheelDeceleration * dt, F64.C0);
}
else if (spinVelocity < F64.C0)
{
spinVelocity = MathHelper.Min(spinVelocity + brakeFreezeWheelDeceleration * dt, F64.C0);
}
}
else if (!wheel.HasSupport)
{
//Just idly slowing down.
if (spinVelocity > F64.C0)
{
spinVelocity = MathHelper.Max(spinVelocity - airborneWheelDeceleration * dt, F64.C0);
}
else if (spinVelocity < F64.C0)
{
spinVelocity = MathHelper.Min(spinVelocity + airborneWheelDeceleration * dt, F64.C0);
}
}
spinAngle += spinVelocity * dt;
}
internal static bool CheckBoundings(MGFObject a, MGFObject b)
{
Fix64Vector2 posA = a.GetPos();
Fix64Vector2 posB = b.GetPos();
return(Fix64.Abs(posA.X - posB.X) < a.GetHalfSize().X + b.GetHalfSize().X&&
Fix64.Abs(posA.Y - posB.Y) < a.GetHalfSize().Y + b.GetHalfSize().Y);
}
public static bool Gauss(Fix64[,] M, int m, int n)
{
// Perform Gauss-Jordan elimination
for (int k = 0; k < m; k++)
{
Fix64 maxValue = Fix64.Abs(M[k, k]);
int iMax = k;
for (int i = k + 1; i < m; i++)
{
Fix64 value = Fix64.Abs(M[i, k]);
if (value >= maxValue)
{
maxValue = value;
iMax = i;
}
}
if (maxValue == F64.C0)
{
return(false);
}
// Swap rows k, iMax
if (k != iMax)
{
for (int j = 0; j < n; j++)
{
Fix64 temp = M[k, j];
M[k, j] = M[iMax, j];
M[iMax, j] = temp;
}
}
// Divide row by pivot
Fix64 pivotInverse = F64.C1 / M[k, k];
M[k, k] = F64.C1;
for (int j = k + 1; j < n; j++)
{
M[k, j] *= pivotInverse;
}
// Subtract row k from other rows
for (int i = 0; i < m; i++)
{
if (i == k)
{
continue;
}
Fix64 f = M[i, k];
for (int j = k + 1; j < n; j++)
{
M[i, j] = M[i, j] - M[k, j] * f;
}
M[i, k] = F64.C0;
}
}
return(true);
}
/// <summary>
/// Computes the angle change represented by a normalized quaternion.
/// </summary>
/// <param name="q">Quaternion to be converted.</param>
/// <returns>Angle around the axis represented by the quaternion.</returns>
public static Fix64 GetAngleFromQuaternion(ref Quaternion q)
{
Fix64 qw = Fix64.Abs(q.W);
if (qw > F64.C1)
{
return(F64.C0);
}
return(F64.C2 * Fix64.Acos(qw));
}
/// <summary>
/// Sets up the axes of the transform and ensures that it is an orthonormal basis.
/// </summary>
/// <param name="matrix">Rotation matrix representing the three axes.
/// The matrix's backward vector is used as the primary axis.
/// The matrix's right vector is used as the x axis.</param>
public void SetLocalAxes(Matrix3x3 matrix)
{
if (Fix64.Abs(Vector3.Dot(matrix.Backward, matrix.Right)) > Toolbox.BigEpsilon)
{
throw new ArgumentException("The axes provided to the joint transform are not perpendicular. Ensure that the specified axes form a valid constraint.");
}
localPrimaryAxis = Vector3.Normalize(matrix.Backward);
localXAxis = Vector3.Normalize(matrix.Right);
ComputeWorldSpaceAxes();
}
/// <summary>
/// Calculates and applies corrective impulses.
/// Called automatically by space.
/// </summary>
public override Fix64 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(Fix64.Abs(lambda.X) +
Fix64.Abs(lambda.Y) +
Fix64.Abs(lambda.Z));
}
/// <summary>
/// Finds and fixes "pinch points," points where two polygon
/// vertices are at the same point.
/// If a pinch point is found, pin is broken up into poutA and poutB
/// and true is returned; otherwise, returns false.
/// Mostly for internal use.
/// O(N^2) time, which sucks...
/// </summary>
/// <param name="pin">The pin.</param>
/// <param name="poutA">The pout A.</param>
/// <param name="poutB">The pout B.</param>
/// <param name="tolerance"></param>
private static bool ResolvePinchPoint(Vertices pin, out Vertices poutA, out Vertices poutB, GGame.Math.Fix64 tolerance)
{
poutA = new Vertices();
poutB = new Vertices();
if (pin.Count < 3)
{
return(false);
}
bool hasPinchPoint = false;
int pinchIndexA = -1;
int pinchIndexB = -1;
for (int i = 0; i < pin.Count; ++i)
{
for (int j = i + 1; j < pin.Count; ++j)
{
//Don't worry about pinch points where the points
//are actually just dupe neighbors
if (Fix64.Abs(pin[i].X - pin[j].X) < tolerance && Fix64.Abs(pin[i].Y - pin[j].Y) < tolerance && j != i + 1)
{
pinchIndexA = i;
pinchIndexB = j;
hasPinchPoint = true;
break;
}
}
if (hasPinchPoint)
{
break;
}
}
if (hasPinchPoint)
{
int sizeA = pinchIndexB - pinchIndexA;
if (sizeA == pin.Count)
{
return(false); //has dupe points at wraparound, not a problem here
}
for (int i = 0; i < sizeA; ++i)
{
int ind = Remainder(pinchIndexA + i, pin.Count); // is this right
poutA.Add(pin[ind]);
}
int sizeB = pin.Count - sizeA;
for (int i = 0; i < sizeB; ++i)
{
int ind = Remainder(pinchIndexB + i, pin.Count); // is this right
poutB.Add(pin[ind]);
}
}
return(hasPinchPoint);
}
/// <summary>
/// Sets up the axes of the transform and ensures that it is an orthonormal basis.
/// </summary>
/// <param name="matrix">Rotation matrix representing the three axes.
/// The matrix's backward vector is used as the primary axis.
/// The matrix's right vector is used as the x axis.</param>
public void SetWorldAxes(Matrix3x3 matrix)
{
if (Fix64.Abs(Vector3.Dot(matrix.Backward, matrix.Right)) > Toolbox.BigEpsilon)
{
throw new ArgumentException("The axes provided to the joint transform are not perpendicular. Ensure that the specified axes form a valid constraint.");
}
primaryAxis = Vector3.Normalize(matrix.Backward);
xAxis = Vector3.Normalize(matrix.Right);
Matrix3x3.TransformTranspose(ref this.primaryAxis, ref rotationMatrix, out localPrimaryAxis);
Matrix3x3.TransformTranspose(ref this.xAxis, ref rotationMatrix, out localXAxis);
}
/// <summary>
/// Changes every sign of the matrix entry to '+'
/// </summary>
/// <param name="matrix">The matrix.</param>
/// <param name="result">The absolute matrix.</param>
#region public static void Absolute(ref JMatrix matrix,out JMatrix result)
public static void Absolute(ref FPMatrix matrix, out FPMatrix result)
{
result.M11 = Fix64.Abs(matrix.M11);
result.M12 = Fix64.Abs(matrix.M12);
result.M13 = Fix64.Abs(matrix.M13);
result.M21 = Fix64.Abs(matrix.M21);
result.M22 = Fix64.Abs(matrix.M22);
result.M23 = Fix64.Abs(matrix.M23);
result.M31 = Fix64.Abs(matrix.M31);
result.M32 = Fix64.Abs(matrix.M32);
result.M33 = Fix64.Abs(matrix.M33);
}
internal bool IsHitUnique(RawList <RayHit> hits, ref RayHit hit)
{
for (int i = 0; i < hits.Count; i++)
{
if (Fix64.Abs(hits.Elements[i].T - hit.T) < MeshHitUniquenessThreshold)
{
return(false);
}
}
hits.Add(hit);
return(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 Fix64 SolveIteration()
{
//Compute the current relative velocity.
Fix64 lambda, dot;
Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out lambda);
Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot);
lambda += dot;
Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot);
lambda += dot;
Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot);
lambda += dot;
//Add in the constraint space bias velocity
lambda = -lambda + biasVelocity - softness * accumulatedImpulse;
//Transform to an impulse
lambda *= massMatrix;
//Clamp accumulated impulse (can't go negative)
Fix64 previousAccumulatedImpulse = accumulatedImpulse;
if (unadjustedError < F64.C0)
{
accumulatedImpulse = MathHelper.Min(accumulatedImpulse + lambda, F64.C0);
}
else
{
accumulatedImpulse = MathHelper.Max(accumulatedImpulse + lambda, F64.C0);
}
lambda = accumulatedImpulse - previousAccumulatedImpulse;
//Apply the impulse
Vector3 impulse;
if (connectionA.isDynamic)
{
Vector3.Multiply(ref jLinearA, lambda, out impulse);
connectionA.ApplyLinearImpulse(ref impulse);
Vector3.Multiply(ref jAngularA, lambda, out impulse);
connectionA.ApplyAngularImpulse(ref impulse);
}
if (connectionB.isDynamic)
{
Vector3.Multiply(ref jLinearB, lambda, out impulse);
connectionB.ApplyLinearImpulse(ref impulse);
Vector3.Multiply(ref jAngularB, lambda, out impulse);
connectionB.ApplyAngularImpulse(ref impulse);
}
return(Fix64.Abs(lambda));
}
public void Abs()
{
Assert.AreEqual(Fix64.MaxValue(), Fix64.Abs(Fix64.MinValue()));
var sources = new[] { -1, 0, 1, int.MaxValue };
var expecteds = new[] { 1, 0, 1, int.MaxValue };
for (int i = 0; i < sources.Length; ++i)
{
var actual = Fix64.Abs((Fix64)sources[i]);
var expected = (Fix64)expecteds[i];
Assert.AreEqual(expected, actual);
}
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.Positions[_indexA].C;
GGame.Math.Fix64 aA = data.Positions[_indexA].A;
Vector2 cB = data.Positions[_indexB].C;
GGame.Math.Fix64 aB = data.Positions[_indexB].A;
Rot qA = new Rot(aA), qB = new Rot(aB);
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 d = (cB - cA) + rB - rA;
Vector2 ay = MathUtils.Mul(qA, _localYAxis);
GGame.Math.Fix64 sAy = MathUtils.Cross(d + rA, ay);
GGame.Math.Fix64 sBy = MathUtils.Cross(rB, ay);
GGame.Math.Fix64 C = Vector2.Dot(d, ay);
GGame.Math.Fix64 k = _invMassA + _invMassB + _invIA * _sAy * _sAy + _invIB * _sBy * _sBy;
GGame.Math.Fix64 impulse;
if (k != 0.0f)
{
impulse = -C / k;
}
else
{
impulse = 0.0f;
}
Vector2 P = impulse * ay;
GGame.Math.Fix64 LA = impulse * sAy;
GGame.Math.Fix64 LB = impulse * sBy;
cA -= _invMassA * P;
aA -= _invIA * LA;
cB += _invMassB * P;
aB += _invIB * LB;
data.Positions[_indexA].C = cA;
data.Positions[_indexA].A = aA;
data.Positions[_indexB].C = cB;
data.Positions[_indexB].A = aB;
return(Fix64.Abs(C) <= Settings.LinearSlop);
}
/// <summary>
/// Cylinder shape used to compute the expanded bounding box of the character.
/// </summary>
void ExpandBoundingBox()
{
if (Body.ActivityInformation.IsActive)
{
//This runs after the bounding box updater is run, but before the broad phase.
//Expanding the character's bounding box ensures that minor variations in velocity will not cause
//any missed information.
//TODO: seems a bit silly to do this work sequentially. Would be better if it could run in parallel in the proper location.
var down = Down;
var boundingBox = Body.CollisionInformation.BoundingBox;
//Expand the bounding box up and down using the step height.
Vector3 expansion;
Vector3.Multiply(ref down, StepManager.MaximumStepHeight, out expansion);
expansion.X = Fix64.Abs(expansion.X);
expansion.Y = Fix64.Abs(expansion.Y);
expansion.Z = Fix64.Abs(expansion.Z);
//When the character climbs a step, it teleports horizontally a little to gain support. Expand the bounding box to accommodate the margin.
//Compute the expansion caused by the extra radius along each axis.
//There's a few ways to go about doing this.
//The following is heavily cooked, but it is based on the angle between the vertical axis and a particular axis.
//Given that, the amount of the radial expansion required along that axis can be computed.
//The dot product would provide the cos(angle) between the vertical axis and a chosen axis.
//Equivalently, it is how much expansion would be along that axis, if the vertical axis was the axis of expansion.
//However, it's not. The dot product actually gives us the expansion along an axis perpendicular to the chosen axis, pointing away from the character's vertical axis.
//What we need is actually given by the sin(angle), which is given by ||verticalAxis x testAxis||.
//The sin(angle) is the projected length of the verticalAxis (not the expansion!) on the axis perpendicular to the testAxis pointing away from the character's vertical axis.
//That projected length, however is equal to the expansion along the test axis, which is exactly what we want.
//To show this, try setting up the triangles at the corner of a cylinder with the world axes and cylinder axes.
//Since the test axes we're using are all standard directions ({0,0,1}, {0,1,0}, and {0,0,1}), most of the cross product logic simplifies out, and we are left with:
var horizontalExpansionAmount = Body.CollisionInformation.Shape.CollisionMargin * F64.C1p1;
Vector3 squaredDown;
squaredDown.X = down.X * down.X;
squaredDown.Y = down.Y * down.Y;
squaredDown.Z = down.Z * down.Z;
expansion.X += horizontalExpansionAmount * Fix64.Sqrt(squaredDown.Y + squaredDown.Z);
expansion.Y += horizontalExpansionAmount * Fix64.Sqrt(squaredDown.X + squaredDown.Z);
expansion.Z += horizontalExpansionAmount * Fix64.Sqrt(squaredDown.X + squaredDown.Y);
Vector3.Add(ref expansion, ref boundingBox.Max, out boundingBox.Max);
Vector3.Subtract(ref boundingBox.Min, ref expansion, out boundingBox.Min);
Body.CollisionInformation.BoundingBox = boundingBox;
}
}
/// <summary>
/// Sets up the axes of the transform and ensures that it is an orthonormal basis.
/// </summary>
/// <param name="primaryAxis">First axis in the transform. Usually aligned along the main axis of a joint, like the twist axis of a TwistLimit.</param>
/// <param name="xAxis">Second axis in the transform.</param>
/// <param name="yAxis">Third axis in the transform.</param>
public void SetLocalAxes(Vector3 primaryAxis, Vector3 xAxis, Vector3 yAxis)
{
if (Fix64.Abs(Vector3.Dot(primaryAxis, xAxis)) > Toolbox.BigEpsilon ||
Fix64.Abs(Vector3.Dot(primaryAxis, yAxis)) > Toolbox.BigEpsilon ||
Fix64.Abs(Vector3.Dot(xAxis, yAxis)) > Toolbox.BigEpsilon)
{
throw new ArgumentException("The axes provided to the joint transform do not form an orthonormal basis. Ensure that each axis is perpendicular to the other two.");
}
localPrimaryAxis = Vector3.Normalize(primaryAxis);
localXAxis = Vector3.Normalize(xAxis);
localYAxis = Vector3.Normalize(yAxis);
ComputeWorldSpaceAxes();
}
/// <summary>
/// Sets up the axes of the transform and ensures that it is an orthonormal basis.
/// </summary>
/// <param name="matrix">Rotation matrix representing the three axes.
/// The matrix's backward vector is used as the primary axis.
/// The matrix's right vector is used as the x axis.
/// The matrix's up vector is used as the y axis.</param>
public void SetLocalAxes(Matrix3x3 matrix)
{
if (Fix64.Abs(Vector3.Dot(matrix.Backward, matrix.Right)) > Toolbox.BigEpsilon ||
Fix64.Abs(Vector3.Dot(matrix.Backward, matrix.Up)) > Toolbox.BigEpsilon ||
Fix64.Abs(Vector3.Dot(matrix.Right, matrix.Up)) > Toolbox.BigEpsilon)
{
throw new ArgumentException("The axes provided to the joint transform do not form an orthonormal basis. Ensure that each axis is perpendicular to the other two.");
}
localPrimaryAxis = Vector3.Normalize(matrix.Backward);
localXAxis = Vector3.Normalize(matrix.Right);
localYAxis = Vector3.Normalize(matrix.Up);
ComputeWorldSpaceAxes();
}
/// <summary>
/// Applies the corrective impulses required by the constraint.
/// </summary>
public override Fix64 SolveIteration()
{
#if !WINDOWS
Vector3 lambda = new Vector3();
#else
Vector3 lambda;
#endif
Vector3 aVel = connectionA.angularVelocity;
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);
Vector3 previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse.X += lambda.X;
accumulatedImpulse.Y += lambda.Y;
accumulatedImpulse.Z += lambda.Z;
Fix64 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.
Fix64 multiplier = maxForceDt / Fix64.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)
{
Vector3 torqueB;
Vector3.Negate(ref lambda, out torqueB);
connectionB.ApplyAngularImpulse(ref torqueB);
}
return(Fix64.Abs(lambda.X) + Fix64.Abs(lambda.Y) + Fix64.Abs(lambda.Z));
}
// From Eric Jordan's convex decomposition library
/// <summary>
/// Check if the lines a0->a1 and b0->b1 cross.
/// If they do, intersectionPoint will be filled
/// with the point of crossing.
/// Grazing lines should not return true.
/// </summary>
public static bool LineIntersect2(ref Vector2 a0, ref Vector2 a1, ref Vector2 b0, ref Vector2 b1, out Vector2 intersectionPoint)
{
intersectionPoint = Vector2.Zero;
if (a0 == b0 || a0 == b1 || a1 == b0 || a1 == b1)
{
return(false);
}
GGame.Math.Fix64 x1 = a0.X;
GGame.Math.Fix64 y1 = a0.Y;
GGame.Math.Fix64 x2 = a1.X;
GGame.Math.Fix64 y2 = a1.Y;
GGame.Math.Fix64 x3 = b0.X;
GGame.Math.Fix64 y3 = b0.Y;
GGame.Math.Fix64 x4 = b1.X;
GGame.Math.Fix64 y4 = b1.Y;
//AABB early exit
if (Math.Max((float)x1, (float)x2) < Math.Min((float)x3, (float)x4) || Math.Max((float)x3, (float)x4) < Math.Min((float)x1, (float)x2))
{
return(false);
}
if (Math.Max((float)y1, (float)y2) < Math.Min((float)y3, (float)y4) || Math.Max((float)y3, (float)y4) < Math.Min((float)y1, (float)y2))
{
return(false);
}
GGame.Math.Fix64 ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3));
GGame.Math.Fix64 ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3));
GGame.Math.Fix64 denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
if (Fix64.Abs(denom) < Settings.Epsilon)
{
//Lines are too close to parallel to call
return(false);
}
ua /= denom;
ub /= denom;
if ((0 < ua) && (ua < 1) && (0 < ub) && (ub < 1))
{
intersectionPoint.X = (x1 + ua * (x2 - x1));
intersectionPoint.Y = (y1 + ua * (y2 - y1));
return(true);
}
return(false);
}
/// <summary>
/// Calculates and applies corrective impulses.
/// Called automatically by space.
/// </summary>
public override Fix64 SolveIteration()
{
Fix64 linearSpeed = entity.linearVelocity.LengthSquared();
if (linearSpeed > maximumSpeedSquared)
{
linearSpeed = Fix64.Sqrt(linearSpeed);
Vector3 impulse;
//divide by linearSpeed to normalize the velocity.
//Multiply by linearSpeed - maximumSpeed to get the 'velocity change vector.'
Vector3.Multiply(ref entity.linearVelocity, -(linearSpeed - maximumSpeed) / linearSpeed, out impulse);
//incorporate softness
Vector3 softnessImpulse;
Vector3.Multiply(ref accumulatedImpulse, usedSoftness, out softnessImpulse);
Vector3.Subtract(ref impulse, ref softnessImpulse, out impulse);
//Transform into impulse
Vector3.Multiply(ref impulse, effectiveMassMatrix, out impulse);
//Accumulate
Vector3 previousAccumulatedImpulse = accumulatedImpulse;
Vector3.Add(ref accumulatedImpulse, ref impulse, out accumulatedImpulse);
Fix64 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.
Fix64 multiplier = maxForceDt / Fix64.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(Fix64.Abs(impulse.X) + Fix64.Abs(impulse.Y) + Fix64.Abs(impulse.Z));
}
return(F64.C0);
}
/// <summary>
/// Sets up the axes of the transform and ensures that it is an orthonormal basis.
/// </summary>
/// <param name="matrix">Rotation matrix representing the three axes.
/// The matrix's backward vector is used as the primary axis.
/// The matrix's right vector is used as the x axis.
/// The matrix's up vector is used as the y axis.</param>
public void SetWorldAxes(Matrix3x3 matrix)
{
if (Fix64.Abs(Vector3.Dot(matrix.Backward, matrix.Right)) > Toolbox.BigEpsilon ||
Fix64.Abs(Vector3.Dot(matrix.Backward, matrix.Up)) > Toolbox.BigEpsilon ||
Fix64.Abs(Vector3.Dot(matrix.Right, matrix.Up)) > Toolbox.BigEpsilon)
{
throw new ArgumentException("The axes provided to the joint transform do not form an orthonormal basis. Ensure that each axis is perpendicular to the other two.");
}
primaryAxis = Vector3.Normalize(matrix.Backward);
xAxis = Vector3.Normalize(matrix.Right);
yAxis = Vector3.Normalize(matrix.Up);
Matrix3x3.TransformTranspose(ref this.primaryAxis, ref rotationMatrix, out localPrimaryAxis);
Matrix3x3.TransformTranspose(ref this.xAxis, ref rotationMatrix, out localXAxis);
Matrix3x3.TransformTranspose(ref this.yAxis, ref rotationMatrix, out localYAxis);
}
// 判断给定扇形是否与指定圆形相交
public static bool IsFunOverlappedCircle(Vec2 circleCenter, Fix64 circleR, Vec2 fanCenter, Fix64 fanR, Fix64 fanDir, Fix64 fanAngle)
{
// 先判断圆心距离
var dc = circleCenter - fanCenter;
var r = dc.Length;
if (r > circleR + fanR)
{
return(false);
}
// 在判断方向角度
var dir = MU.v2Degree(dc.x, dc.y);
var dd = (dir - fanDir).RangeIn180();
return(Fix64.Abs(dd) <= fanAngle / 2);
}
private Fix64 minValueFunc(Fix64 a, Fix64 b, Fix64 c, Fix64 l1, Fix64 l2, Fix64 aDelta, out Fix64 et)
{
if (Fix64.Abs(a) <= aDelta)
{
var r1 = b * l1 + c;
var r2 = b * l2 + c;
et = (r1 < r2) ? l1 : l2;
return((r1 < r2) ? r1 : r2);
}
if (a > Fix64.Zero)
{
var ext = -b / ((Fix64)2 * a);
if (ext < l1)
{
et = l1;
return(a * l1 * l1 + b * l1 + c);
}
if (ext > l2)
{
et = l2;
return(a * l2 * l2 + b * l2 + c);
}
et = ext;
return(a * ext * ext + b * ext + c);
}
if (a < Fix64.Zero)
{
var ext = -b / ((Fix64)2 * a);
if (ext < l1)
{
et = l2;
return(a * l2 * l2 + b * l2 + c);
}
if (ext > l2)
{
et = l1;
return(a * l1 * l1 + b * l1 + c);
}
var r1 = a * l2 * l2 + b * l2 + c;
var r2 = a * l1 * l1 + b * l1 + c;
et = (r1 < r2) ? l1 : l2;
return((r1 < r2) ? r1 : r2);
}
et = Fix64.Zero;
return((Fix64)0);
}
/// <summary>
/// Determines if and when the ray intersects the plane.
/// </summary>
/// <param name="plane">Plane to test against.</param>
/// <param name="t">The length along the ray to the impact, if any impact occurs.</param>
/// <returns>True if the ray intersects the target, false otherwise.</returns>
public bool Intersects(ref Plane plane, out Fix64 t)
{
Fix64 velocity;
Vector3.Dot(ref Direction, ref plane.Normal, out velocity);
if (Fix64.Abs(velocity) < Toolbox.Epsilon)
{
t = F64.C0;
return(false);
}
Fix64 distanceAlongNormal;
Vector3.Dot(ref Position, ref plane.Normal, out distanceAlongNormal);
distanceAlongNormal += plane.D;
t = -distanceAlongNormal / velocity;
return(t >= -Toolbox.Epsilon);
}
/// <summary>
/// Computes one iteration of the constraint to meet the solver updateable's goal.
/// </summary>
/// <returns>The rough applied impulse magnitude.</returns>
public override Fix64 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.
Fix64 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 / Fix64.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(Fix64.Abs(lambda.X) + Fix64.Abs(lambda.Y) + Fix64.Abs(lambda.Z));
}
/// <summary>
/// Computes one iteration of the constraint to meet the solver updateable's goal.
/// </summary>
/// <returns>The rough applied impulse magnitude.</returns>
public override Fix64 SolveIteration()
{
//Compute relative velocity and convert to impulse
Fix64 lambda = RelativeVelocity * velocityToImpulse;
//Clamp accumulated impulse
Fix64 previousAccumulatedImpulse = accumulatedImpulse;
Fix64 maxForce = friction * penetrationConstraint.accumulatedImpulse;
accumulatedImpulse = MathHelper.Clamp(accumulatedImpulse + lambda, -maxForce, maxForce);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
//Apply the impulse
#if !WINDOWS
Vector3 linear = new Vector3();
Vector3 angular = new Vector3();
#else
Vector3 linear, angular;
#endif
linear.X = lambda * linearAX;
linear.Y = lambda * linearAY;
linear.Z = lambda * linearAZ;
if (entityAIsDynamic)
{
angular.X = lambda * angularAX;
angular.Y = lambda * angularAY;
angular.Z = lambda * angularAZ;
entityA.ApplyLinearImpulse(ref linear);
entityA.ApplyAngularImpulse(ref angular);
}
if (entityBIsDynamic)
{
linear.X = -linear.X;
linear.Y = -linear.Y;
linear.Z = -linear.Z;
angular.X = lambda * angularBX;
angular.Y = lambda * angularBY;
angular.Z = lambda * angularBZ;
entityB.ApplyLinearImpulse(ref linear);
entityB.ApplyAngularImpulse(ref angular);
}
return(Fix64.Abs(lambda));
}
