public override void GenPatches(GameObject parent,
Vector2f origin,
Vector2f center,
int numChunks,
int numEntityPerChunk,
float parentSize)
{
float chunkSize = parentSize / ((float)numChunks);
Vector2f max = new Vector2f((numChunks) * chunkSize);
max.DivideStore(2);
for (int x = 0; x < numChunks; x++)
{
for (int z = 0; z < numChunks; z++)
{
Vector2f offset = new Vector2f(x * chunkSize, z * chunkSize);
Vector2f chunkLoc = origin.Add(offset).SubtractStore(max);
GridTile tile = new GridTile(chunkLoc, chunkSize, chunkSize, chunkLoc.x, chunkLoc.y, 85f);
Patch patch = new Patch((Node)parent, tile);
patch.translation = new Vector3f(chunkLoc.x, 0, chunkLoc.y);
patch.chunkSize = chunkSize;
patch.entityPerChunk = numEntityPerChunk;
patches.Add(patch);
}
}
}
public static Vector2f GetVelocity(Vector2f velocity, List <Vector2f> forces)
{
foreach (Vector2f v in forces)
{
velocity.Add(v);
}
return(velocity);
}
public static Vector2f GetVelocity(Vector2f velocity, Vector2f force,
float mass)
{
Vector2f acceleration = new Vector2f(force.GetX() / mass, force.GetY()
/ mass);
velocity.Add(acceleration);
return(velocity);
}
/// <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()
{
// lambda = -mc * (Jv + b)
// P = JT * lambda
Vector3f velocity;
Vector3f.Subtract(ref connectionA.angularVelocity, ref connectionB.angularVelocity, out velocity);
#if !WINDOWS
Vector2f lambda = new Vector2f();
#else
Vector2f lambda;
#endif
Vector3f.Dot(ref worldConstrainedAxis1, ref velocity, out lambda.X);
Vector3f.Dot(ref worldConstrainedAxis2, ref velocity, out lambda.Y);
Vector2f.Add(ref lambda, ref biasVelocity, out lambda);
Vector2f softnessImpulse;
Vector2f.Multiply(ref accumulatedImpulse, softness, out softnessImpulse);
Vector2f.Add(ref lambda, ref softnessImpulse, out lambda);
Vector2f.Transform(ref lambda, ref effectiveMassMatrix, out lambda);
Vector2f.Add(ref accumulatedImpulse, ref lambda, out accumulatedImpulse);
#if !WINDOWS
Vector3f impulse = new Vector3f();
#else
Vector3f impulse;
#endif
impulse.X = worldConstrainedAxis1.X * lambda.X + worldConstrainedAxis2.X * lambda.Y;
impulse.Y = worldConstrainedAxis1.Y * lambda.X + worldConstrainedAxis2.Y * lambda.Y;
impulse.Z = worldConstrainedAxis1.Z * lambda.X + worldConstrainedAxis2.Z * lambda.Y;
if (connectionA.isDynamic)
{
connectionA.ApplyAngularImpulse(ref impulse);
}
if (connectionB.isDynamic)
{
Vector3f.Negate(ref impulse, out impulse);
connectionB.ApplyAngularImpulse(ref impulse);
}
return(Math.Abs(lambda.X) + Math.Abs(lambda.Y));
}
/// <summary>
/// Computes a solution to the constraint.
/// </summary>
/// <returns>Impulse magnitude computed by the iteration.</returns>
public override float SolveIteration()
{
Vector2f relativeVelocity = RelativeVelocity;
Vector2f.Add(ref relativeVelocity, ref positionCorrectionBias, out relativeVelocity);
//Create the full velocity change, and convert it to an impulse in constraint space.
Vector2f lambda;
Vector2f.Subtract(ref targetVelocity, ref relativeVelocity, out lambda);
Vector2f.Transform(ref lambda, ref massMatrix, out lambda);
//Add and clamp the impulse.
Vector2f previousAccumulatedImpulse = accumulatedImpulse;
if (MovementMode == MovementMode.Floating)
{
//If it's floating, clamping rules are different.
//The constraint is not permitted to slow down the character; only speed it up.
//This offers a hole for an exploit; by jumping and curving just right,
//the character can accelerate beyond its maximum speed. A bit like an HL2 speed run.
accumulatedImpulse.X = MathHelper.Clamp(accumulatedImpulse.X + lambda.X, 0, maxForceDt);
accumulatedImpulse.Y = 0;
}
else
{
Vector2f.Add(ref lambda, ref accumulatedImpulse, out accumulatedImpulse);
float length = accumulatedImpulse.LengthSquared;
if (length > maxForceDt * maxForceDt)
{
Vector2f.Multiply(ref accumulatedImpulse, maxForceDt / (float)Math.Sqrt(length), out accumulatedImpulse);
}
if (isTryingToMove && accumulatedImpulse.X > maxAccelerationForceDt)
{
accumulatedImpulse.X = maxAccelerationForceDt;
}
}
Vector2f.Subtract(ref accumulatedImpulse, ref previousAccumulatedImpulse, out lambda);
//Use the jacobians to put the impulse into world space.
#if !WINDOWS
Vector3f impulse = new Vector3f();
Vector3f torque = new Vector3f();
#else
Vector3f impulse;
Vector3f torque;
#endif
float x = lambda.X;
float y = lambda.Y;
impulse.X = linearJacobianA1.X * x + linearJacobianA2.X * y;
impulse.Y = linearJacobianA1.Y * x + linearJacobianA2.Y * y;
impulse.Z = linearJacobianA1.Z * x + linearJacobianA2.Z * y;
characterBody.ApplyLinearImpulse(ref impulse);
if (supportEntity != null && supportEntity.IsDynamic)
{
Vector3f.Multiply(ref impulse, -supportForceFactor, out impulse);
x *= supportForceFactor;
y *= supportForceFactor;
torque.X = x * angularJacobianB1.X + y * angularJacobianB2.X;
torque.Y = x * angularJacobianB1.Y + y * angularJacobianB2.Y;
torque.Z = x * angularJacobianB1.Z + y * angularJacobianB2.Z;
supportEntity.ApplyLinearImpulse(ref impulse);
supportEntity.ApplyAngularImpulse(ref torque);
}
return(Math.Abs(lambda.X) + Math.Abs(lambda.Y));
}
public static Vector2f GetVelocity(Vector2f velocity, Vector2f force)
{
velocity.Add(force);
return(velocity);
}
/// <summary>
/// Gets the intersection between the convex shape and the ray.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="transform">Transform of the convex shape.</param>
/// <param name="maximumLength">Maximum distance to travel in units of the ray direction's length.</param>
/// <param name="hit">Ray hit data, if any.</param>
/// <returns>Whether or not the ray hit the target.</returns>
public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit)
{
//Put the ray into local space.
Quaternion conjugate;
Quaternion.Conjugate(ref transform.Orientation, out conjugate);
Ray localRay;
Vector3f.Subtract(ref ray.Position, ref transform.Position, out localRay.Position);
Vector3f.Transform(ref localRay.Position, ref conjugate, out localRay.Position);
Vector3f.Transform(ref ray.Direction, ref conjugate, out localRay.Direction);
//Check for containment in the cylindrical portion of the capsule.
if (localRay.Position.Y >= -halfLength && localRay.Position.Y <= halfLength && localRay.Position.X * localRay.Position.X + localRay.Position.Z * localRay.Position.Z <= collisionMargin * collisionMargin)
{
//It's inside!
hit.T = 0;
hit.Location = localRay.Position;
hit.Normal = new Vector3f(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared;
if (normalLengthSquared > 1e-9f)
{
Vector3f.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
}
else
{
hit.Normal = new Vector3f();
}
//Pull the hit into world space.
Vector3f.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location);
return(true);
}
//Project the ray direction onto the plane where the cylinder is a circle.
//The projected ray is then tested against the circle to compute the time of impact.
//That time of impact is used to compute the 3d hit location.
Vector2f planeDirection = new Vector2f(localRay.Direction.X, localRay.Direction.Z);
float planeDirectionLengthSquared = planeDirection.LengthSquared;
if (planeDirectionLengthSquared < MathHelper.Epsilon)
{
//The ray is nearly parallel with the axis.
//Skip the cylinder-sides test. We're either inside the cylinder and won't hit the sides, or we're outside
//and won't hit the sides.
if (localRay.Position.Y > halfLength)
{
goto upperSphereTest;
}
if (localRay.Position.Y < -halfLength)
{
goto lowerSphereTest;
}
hit = new RayHit();
return(false);
}
Vector2f planeOrigin = new Vector2f(localRay.Position.X, localRay.Position.Z);
float dot;
Vector2f.Dot(ref planeDirection, ref planeOrigin, out dot);
float closestToCenterT = -dot / planeDirectionLengthSquared;
Vector2f closestPoint;
Vector2f.Multiply(ref planeDirection, closestToCenterT, out closestPoint);
Vector2f.Add(ref planeOrigin, ref closestPoint, out closestPoint);
//How close does the ray come to the circle?
float squaredDistance = closestPoint.LengthSquared;
if (squaredDistance > collisionMargin * collisionMargin)
{
//It's too far! The ray cannot possibly hit the capsule.
hit = new RayHit();
return(false);
}
//With the squared distance, compute the distance backward along the ray from the closest point on the ray to the axis.
float backwardsDistance = collisionMargin * (float)Math.Sqrt(1 - squaredDistance / (collisionMargin * collisionMargin));
float tOffset = backwardsDistance / (float)Math.Sqrt(planeDirectionLengthSquared);
hit.T = closestToCenterT - tOffset;
//Compute the impact point on the infinite cylinder in 3d local space.
Vector3f.Multiply(ref localRay.Direction, hit.T, out hit.Location);
Vector3f.Add(ref hit.Location, ref localRay.Position, out hit.Location);
//Is it intersecting the cylindrical portion of the capsule?
if (hit.Location.Y <= halfLength && hit.Location.Y >= -halfLength && hit.T < maximumLength)
{
//Yup!
hit.Normal = new Vector3f(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared;
if (normalLengthSquared > 1e-9f)
{
Vector3f.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
}
else
{
hit.Normal = new Vector3f();
}
//Pull the hit into world space.
Vector3f.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location);
return(true);
}
if (hit.Location.Y < halfLength)
{
goto lowerSphereTest;
}
upperSphereTest:
//Nope! It may be intersecting the ends of the capsule though.
//We're above the capsule, so cast a ray against the upper sphere.
//We don't have to worry about it hitting the bottom of the sphere since it would have hit the cylinder portion first.
var spherePosition = new Vector3f(0, halfLength, 0);
if (Toolbox.RayCastSphere(ref localRay, ref spherePosition, collisionMargin, maximumLength, out hit))
{
//Pull the hit into world space.
Vector3f.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location);
return(true);
}
//No intersection! We can't be hitting the other sphere, so it's over!
hit = new RayHit();
return(false);
lowerSphereTest:
//Okay, what about the bottom sphere?
//We're above the capsule, so cast a ray against the upper sphere.
//We don't have to worry about it hitting the bottom of the sphere since it would have hit the cylinder portion first.
spherePosition = new Vector3f(0, -halfLength, 0);
if (Toolbox.RayCastSphere(ref localRay, ref spherePosition, collisionMargin, maximumLength, out hit))
{
//Pull the hit into world space.
Vector3f.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location);
return(true);
}
//No intersection! We can't be hitting the other sphere, so it's over!
hit = new RayHit();
return(false);
}
/// <summary>
/// Calculates and applies corrective impulses.
/// Called automatically by space.
/// </summary>
public override float SolveIteration()
{
#region Theory
//lambda = -mc * (Jv + b)
// PraT = [ bx by bz ] * [ 0 raz -ray ] = [ (-by * raz + bz * ray) (bx * raz - bz * rax) (-bx * ray + by * rax) ]
// [ cx cy cz ] [ -raz 0 rax ] [ (-cy * raz + cz * ray) (cx * raz - cz * rax) (-cx * ray + cy * rax) ]
// [ ray -rax 0 ]
//
// PrbT = [ bx by bz ] * [ 0 rbz -rby ] = [ (-by * rbz + bz * rby) (bx * rbz - bz * rbx) (-bx * rby + by * rbx) ]
// [ cx cy cz ] [ -rbz 0 rbx ] [ (-cy * rbz + cz * rby) (cx * rbz - cz * rbx) (-cx * rby + cy * rbx) ]
// [ rby -rbx 0 ]
// Jv = [ bx by bz PraT -bx -by -bz -Prbt ] * [ vax ]
// [ cx cy cz -cx -cy -cz ] [ vay ]
// [ vaz ]
// [ wax ]
// [ way ]
// [ waz ]
// [ vbx ]
// [ vby ]
// [ vbz ]
// [ wbx ]
// [ wby ]
// [ wbz ]
// va' = [ bx * vax + by * vay + bz * vaz ] = [ b * va ]
// [ cx * vax + cy * vay + cz * vaz ] [ c * va ]
// wa' = [ (PraT row 1) * wa ]
// [ (PraT row 2) * wa ]
// vb' = [ -bx * vbx - by * vby - bz * vbz ] = [ -b * vb ]
// [ -cx * vbx - cy * vby - cz * vbz ] [ -c * vb ]
// wb' = [ -(PrbT row 1) * wb ]
// [ -(PrbT row 2) * wb ]
// Jv = [ b * va + (PraT row 1) * wa - b * vb - (PrbT row 1) * wb ]
// [ c * va + (PraT row 2) * wa - c * vb - (PrbT row 2) * wb ]
// Jv = [ b * (va + wa x ra - vb - wb x rb) ]
// [ c * (va + wa x ra - vb - wb x rb) ]
//P = JT * lambda
#endregion
#if !WINDOWS
Vector2f lambda = new Vector2f();
#else
Vector2f lambda;
#endif
//float va1, va2, wa1, wa2, vb1, vb2, wb1, wb2;
//Vector3f.Dot(ref worldAxis1, ref myParentA.myInternalLinearVelocity, out va1);
//Vector3f.Dot(ref worldAxis2, ref myParentA.myInternalLinearVelocity, out va2);
//wa1 = prAT.M11 * myParentA.myInternalAngularVelocity.X + prAT.M12 * myParentA.myInternalAngularVelocity.Y + prAT.M13 * myParentA.myInternalAngularVelocity.Z;
//wa2 = prAT.M21 * myParentA.myInternalAngularVelocity.X + prAT.M22 * myParentA.myInternalAngularVelocity.Y + prAT.M23 * myParentA.myInternalAngularVelocity.Z;
//Vector3f.Dot(ref worldAxis1, ref myParentB.myInternalLinearVelocity, out vb1);
//Vector3f.Dot(ref worldAxis2, ref myParentB.myInternalLinearVelocity, out vb2);
//wb1 = prBT.M11 * myParentB.myInternalAngularVelocity.X + prBT.M12 * myParentB.myInternalAngularVelocity.Y + prBT.M13 * myParentB.myInternalAngularVelocity.Z;
//wb2 = prBT.M21 * myParentB.myInternalAngularVelocity.X + prBT.M22 * myParentB.myInternalAngularVelocity.Y + prBT.M23 * myParentB.myInternalAngularVelocity.Z;
//lambda.X = va1 + wa1 - vb1 - wb1 + biasVelocity.X + mySoftness * accumulatedImpulse.X;
//lambda.Y = va2 + wa2 - vb2 - wb2 + biasVelocity.Y + mySoftness * accumulatedImpulse.Y;
Vector3f dv;
Vector3f aVel, bVel;
Vector3f.Cross(ref connectionA.angularVelocity, ref rA, out aVel);
Vector3f.Add(ref aVel, ref connectionA.linearVelocity, out aVel);
Vector3f.Cross(ref connectionB.angularVelocity, ref rB, out bVel);
Vector3f.Add(ref bVel, ref connectionB.linearVelocity, out bVel);
Vector3f.Subtract(ref aVel, ref bVel, out dv);
Vector3f.Dot(ref dv, ref worldRestrictedAxis1, out lambda.X);
Vector3f.Dot(ref dv, ref worldRestrictedAxis2, out lambda.Y);
lambda.X += biasVelocity.X + softness * accumulatedImpulse.X;
lambda.Y += biasVelocity.Y + softness * accumulatedImpulse.Y;
//Convert to impulse
Vector2f.Transform(ref lambda, ref negativeEffectiveMassMatrix, out lambda);
Vector2f.Add(ref lambda, ref accumulatedImpulse, out accumulatedImpulse);
float x = lambda.X;
float y = lambda.Y;
//Apply impulse
#if !WINDOWS
Vector3f impulse = new Vector3f();
Vector3f torque = new Vector3f();
#else
Vector3f impulse;
Vector3f torque;
#endif
impulse.X = worldRestrictedAxis1.X * x + worldRestrictedAxis2.X * y;
impulse.Y = worldRestrictedAxis1.Y * x + worldRestrictedAxis2.Y * y;
impulse.Z = worldRestrictedAxis1.Z * x + worldRestrictedAxis2.Z * y;
if (connectionA.isDynamic)
{
torque.X = x * angularA1.X + y * angularA2.X;
torque.Y = x * angularA1.Y + y * angularA2.Y;
torque.Z = x * angularA1.Z + y * angularA2.Z;
connectionA.ApplyLinearImpulse(ref impulse);
connectionA.ApplyAngularImpulse(ref torque);
}
if (connectionB.isDynamic)
{
impulse.X = -impulse.X;
impulse.Y = -impulse.Y;
impulse.Z = -impulse.Z;
torque.X = x * angularB1.X + y * angularB2.X;
torque.Y = x * angularB1.Y + y * angularB2.Y;
torque.Z = x * angularB1.Z + y * angularB2.Z;
connectionB.ApplyLinearImpulse(ref impulse);
connectionB.ApplyAngularImpulse(ref torque);
}
return(Math.Abs(lambda.X) + Math.Abs(lambda.Y));
}
