/// <summary>
/// Get the position so that the object does not collide with the object anymore.
/// Important: this is used raycasting checks, as this needs some more corner-cases.
/// </summary>
/// <param name="collider">The collider which is the player</param>
/// <param name="collisionPoint">Point of the collision in the scene</param>
/// <param name="hitNormal">Collision-normal of the object with which the player collided</param>
/// <returns>Position which is possible for the collider so there is no collision.</returns>
private JVector GetPosition(Collider collider, JVector collisionPoint, JVector hitNormal)
{
JVector bbSize = collider.BoundingBoxSize;
GameObject gameObject = collider.GameObject;
// Calculate the size of the forward and right vector based on the bounding-box.
JVector forward = 0.5f * bbSize.Z * Conversion.ToJitterVector(gameObject.transform.Forward);
JVector right = 0.5f * bbSize.X * Conversion.ToJitterVector(gameObject.transform.Right);
JVector d = Conversion.ToJitterVector(gameObject.transform.Forward);
if (hitNormal.IsZero())
{
return(bbSize.Z * -0.5f * Conversion.ToJitterVector(gameObject.transform.Forward));
}
// Correct directions
TurnIntoDirectionOf(ref forward, hitNormal);
TurnIntoDirectionOf(ref right, hitNormal);
TurnIntoDirectionOf(ref d, hitNormal);
// Calculate the projected length (half-length) of the player on the hit-normal.
JVector lengthOnNormal = ProjectOn(forward, hitNormal);
JVector widthOnNormal = ProjectOn(right, hitNormal);
JVector projectedSize = lengthOnNormal + widthOnNormal;
JVector margin = ProjectOn(projectedSize, d);
JVector endPosition = collisionPoint + margin;
// Check if it hit a corner and it could actually go closer
JVector plane = new JVector(-hitNormal.Z, 0, hitNormal.X);
TurnIntoDirectionOf(ref plane, -1 * forward);
plane.Normalize();
// Check if there is an intersection between the front-"plane" of the player and the collided object
JVector intersection;
JVector frontMiddle = endPosition + forward;
JVector frontRight = endPosition + forward - right;
JVector planeEnd = collisionPoint + MathHelper.Max(bbSize.X, bbSize.Z) * plane;
if (DoIntersect(frontMiddle, frontRight, collisionPoint, planeEnd, out intersection))
{
intersection.Y = collider.Position.Y;
JVector colToIntersection = intersection - collisionPoint;
JVector margin2 = ProjectOn(colToIntersection, d);
margin = margin.LengthSquared() > margin2.LengthSquared() ? margin2 : margin;
endPosition = collisionPoint + margin;
}
//// Todo: Visual debuggin might be removed in the end
//PhysicsDrawer.Instance.ClearPointsToDraw();
//PhysicsDrawer.Instance.AddPointToDraw(Conversion.ToXnaVector(collisionPoint));
//PhysicsDrawer.Instance.AddPointToDraw(Conversion.ToXnaVector(endPosition));
//return collider.Position;
return(endPosition);
}
public JVector CalcVelocity(float speed)
{
var state = Keyboard.GetState(); // Does not implemented in .Net core version of OpenTK
var direction = new JVector();
if (state.IsKeyDown(Key.W) || state.IsKeyDown(Key.Up))
{
direction.Z -= 1;
}
if (state.IsKeyDown(Key.S) || state.IsKeyDown(Key.Down))
{
direction.Z += 1;
}
if (state.IsKeyDown(Key.A) || state.IsKeyDown(Key.Left))
{
direction.X -= 1;
}
if (state.IsKeyDown(Key.D) || state.IsKeyDown(Key.Right))
{
direction.X += 1;
}
if (direction != JVector.Zero)
{
return(JVector.Normalize(direction) * speed);
}
else
{
return(direction);
}
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref JVector direction, out JVector result)
{
JVector expandVector;
JVector.Normalize(ref direction, out expandVector);
JVector.Multiply(ref expandVector, sphericalExpansion, out expandVector);
int minIndex = 0;
float min = JVector.Dot(ref points[0], ref direction);
float dot = JVector.Dot(ref points[1], ref direction);
if (dot > min)
{
min = dot;
minIndex = 1;
}
dot = JVector.Dot(ref points[2], ref direction);
if (dot > min)
{
min = dot;
minIndex = 2;
}
JVector.Add(ref points[minIndex], ref expandVector, out result);
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref JVector direction, out JVector result)
{
JVector exp;
JVector.Normalize(ref direction, out exp);
exp *= sphericalExpansion;
float min = JVector.Dot(ref vecs[0], ref direction);
int minIndex = 0;
float dot = JVector.Dot(ref vecs[1], ref direction);
if (dot > min)
{
min = dot;
minIndex = 1;
}
dot = JVector.Dot(ref vecs[2], ref direction);
if (dot > min)
{
min = dot;
minIndex = 2;
}
result = vecs[minIndex] + exp;
}
/// <summary>
/// Normalizes the given vector.
/// </summary>
/// <param name="value">The vector which should be normalized.</param>
/// <returns>A normalized vector.</returns>
#region public static JVector Normalize(JVector value)
public static JVector Normalize(JVector value)
{
JVector result;
JVector.Normalize(ref value, out result);
return(result);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(float timestep)
{
JVector p1, dp;
JVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
JVector.Add(ref body1.position, ref r1, out p1);
JVector.Subtract(ref p1, ref anchor, out dp);
float deltaLength = dp.Length();
JVector n = anchor - p1;
if (n.LengthSquared() != 0.0f)
{
n.Normalize();
}
jacobian[0] = -1.0f * n;
jacobian[1] = -1.0f * (r1 % n);
effectiveMass = body1.inverseMass + JVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = 1.0f / effectiveMass;
bias = deltaLength * biasFactor * (1.0f / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref JVector direction, out JVector result)
{
result = direction;
result.Normalize();
JVector.Multiply(ref result, radius, out result);
}
public void SupportMapping(ref JVector direction, out JVector result)
{
float min = JVector.Dot(ref owner.points[indices.I0].position, ref direction);
float dot = JVector.Dot(ref owner.points[indices.I1].position, ref direction);
JVector minVertex = owner.points[indices.I0].position;
if (dot > min)
{
min = dot;
minVertex = owner.points[indices.I1].position;
}
dot = JVector.Dot(ref owner.points[indices.I2].position, ref direction);
if (dot > min)
{
min = dot;
minVertex = owner.points[indices.I2].position;
}
JVector exp;
JVector.Normalize(ref direction, out exp);
exp *= owner.triangleExpansion;
result = minVertex + exp;
}
public void HandleInput(GameTime gameTime)
{
currentTime += (float)gameTime.ElapsedGameTime.Milliseconds;
KeyboardState keys = Keyboard.GetState();
if (keys.IsKeyDown(Keys.Up))
{
float x = (float)Math.Sin(facing);
float z = (float)Math.Cos(facing);
JVector newPath = new JVector(x, 0f, z) * speed;
newPath = newPath * currentTime;
newPath.Normalize();
forward = newPath;
body.AddForce(newPath * 100f);
//position = body.Position;
//position += newPath;
//body.Position = position;
}
if (keys.IsKeyDown(Keys.Left))
{
facing -= 0.05f;
}
if (keys.IsKeyDown(Keys.Right))
{
facing += 0.05f;
}
if (keys.IsKeyDown(Keys.Down))
{
float x = (float)Math.Sin(facing);
float z = (float)Math.Cos(facing);
JVector newPath = new JVector(x, 0f, z) * speed;
newPath = newPath * currentTime;
newPath.Normalize();
body.AddForce(newPath * -100f);
}
if (keys.IsKeyDown(Keys.Space))
{
body.AddForce(JVector.Up * 100f);
}
/*if (keys.IsKeyDown(Keys.A))
*
* if (keys.IsKeyDown(Keys.S))
*
* if (keys.IsKeyDown(Keys.W))
*/
}
/// <summary>
/// Initializes a contact.
/// </summary>
/// <param name="body1">The first body.</param>
/// <param name="body2">The second body.</param>
/// <param name="point1">The collision point in worldspace</param>
/// <param name="point2">The collision point in worldspace</param>
/// <param name="n">The normal pointing to body2.</param>
/// <param name="penetration">The estimated penetration depth.</param>
public void Initialize(RigidBody body1, RigidBody body2, ref JVector point1, ref JVector point2, ref JVector n,
float penetration, bool newContact, ContactSettings settings)
{
this.body1 = body1; this.body2 = body2;
this.normal = n; normal.Normalize();
this.p1 = point1; this.p2 = point2;
this.newContact = newContact;
JVector.Subtract(ref p1, ref body1.position, out relativePos1);
JVector.Subtract(ref p2, ref body2.position, out relativePos2);
JMatrix o1 = JMatrix.CreateRotationZ(body1.invOrientation);
JMatrix o2 = JMatrix.CreateRotationZ(body2.invOrientation);
JVector.Transform(ref relativePos1, ref o1, out realRelPos1);
JVector.Transform(ref relativePos2, ref o2, out realRelPos2);
this.initialPen = penetration;
this.penetration = penetration;
body1IsMassPoint = body1.isParticle;
body2IsMassPoint = body2.isParticle;
// Material Properties
if (newContact)
{
treatBody1AsStatic = body1.isStatic;
treatBody2AsStatic = body2.isStatic;
accumulatedNormalImpulse = 0.0f;
accumulatedTangentImpulse = 0.0f;
lostSpeculativeBounce = 0.0f;
switch (settings.MaterialCoefficientMixing)
{
case ContactSettings.MaterialCoefficientMixingType.TakeMaximum:
staticFriction = JMath.Max(body1.material.staticFriction, body2.material.staticFriction);
dynamicFriction = JMath.Max(body1.material.kineticFriction, body2.material.kineticFriction);
restitution = JMath.Max(body1.material.restitution, body2.material.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.TakeMinimum:
staticFriction = JMath.Min(body1.material.staticFriction, body2.material.staticFriction);
dynamicFriction = JMath.Min(body1.material.kineticFriction, body2.material.kineticFriction);
restitution = JMath.Min(body1.material.restitution, body2.material.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.UseAverage:
staticFriction = (body1.material.staticFriction + body2.material.staticFriction) / 2.0f;
dynamicFriction = (body1.material.kineticFriction + body2.material.kineticFriction) / 2.0f;
restitution = (body1.material.restitution + body2.material.restitution) / 2.0f;
break;
}
}
this.settings = settings;
}
public Pax4ConstraintBodyVector(Pax4ObjectPhysicsPart p_physicsPart, float p_velocityFactor, Vector3 p_bodyVector)
: base(p_physicsPart._body, null)
{
_velocityFactor = p_velocityFactor;
_bodyVector = Pax4Tools.ToJVector(p_bodyVector);
_bodyVector.Normalize();
SetPhysicsPart(p_physicsPart);
}
public PointOnLine(RigidBody body1, RigidBody body2, JVector lineStartPointBody1, JVector pointBody2) : base(body1, body2)
{
JVector.Subtract(lineStartPointBody1, body1.position, out localAnchor1);
JVector.Subtract(pointBody2, body2.position, out localAnchor2);
JVector.Transform(localAnchor1, body1.invOrientation, out localAnchor1);
JVector.Transform(localAnchor2, body2.invOrientation, out localAnchor2);
lineNormal = JVector.Normalize(lineStartPointBody1 - pointBody2);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The simulation timestep</param>
public override void PrepareForIteration(double timestep)
{
JVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
JVector.Transform(ref localAnchor2, ref body2.orientation, out r2);
JVector p1, p2, dp;
JVector.Add(ref body1.position, ref r1, out p1);
JVector.Add(ref body2.position, ref r2, out p2);
JVector.Subtract(ref p2, ref p1, out dp);
JVector l = JVector.Transform(lineNormal, body1.orientation);
l.Normalize();
JVector t = (p1 - p2) % l;
if (t.LengthSquared() != 0.0f)
{
t.Normalize();
}
t = t % l;
jacobian[0] = t; // linearVel Body1
jacobian[1] = (r1 + p2 - p1) % t; // angularVel Body1
jacobian[2] = -1.0f * t; // linearVel Body2
jacobian[3] = -1.0f * r2 % t; // angularVel Body2
effectiveMass = body1.inverseMass + body2.inverseMass
+ JVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ JVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
if (effectiveMass != 0)
{
effectiveMass = 1.0f / effectiveMass;
}
bias = -(l % (p2 - p1)).Length() * biasFactor * (1.0f / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2];
body2.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
public virtual void Update(GameTime gameTime)
{
Cell cellat = area.CellAt(Position);
if (cellat != null && Position.Y < area.WaterHeight && cellat.isLake)
{
JVector d = area.Physics.Gravity * -1;
d.Normalize();
RigidBody.AddForce(d * (RigidBody.Mass * Bouyancy * area.Physics.Gravity.Length()));
}
}
/// <summary>
/// Reflect the <paramref name="input"/>-vector on the <paramref name="normal"/>-vector.
/// </summary>
/// <param name="input">Input-vector</param>
/// <param name="normal">Reflector-normal</param>
/// <returns>Vector which is the reflection of input on normal</returns>
private JVector ReflectOnNormal(JVector input, JVector normal)
{
JVector normalNormalized = normal;
normalNormalized.Normalize();
JVector inputNormalized = input;
inputNormalized.Normalize();
return(inputNormalized - 2 * JVector.Dot(inputNormalized, normalNormalized) * normalNormalized);
}
public override void PrepareForIteration(float timestep)
{
JVector.Transform(localAnchor1, body1.orientation, out r1);
JVector.Transform(localAnchor2, body2.orientation, out r2);
JVector.Add(body1.position, r1, out var p1);
JVector.Add(body2.position, r2, out var p2);
JVector.Subtract(p2, p1, out _);
var l = JVector.Transform(lineNormal, body1.orientation);
l = JVector.Normalize(l);
var t = (p1 - p2) % l;
if (t.LengthSquared() != 0.0f)
{
t = JVector.Normalize(t);
}
t %= l;
jacobian[0] = t;
jacobian[1] = (r1 + p2 - p1) % t;
jacobian[2] = -1.0f * t;
jacobian[3] = -1.0f * r2 % t;
effectiveMass = body1.inverseMass + body2.inverseMass
+ (JVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1])
+ (JVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3]);
softnessOverDt = Softness / timestep;
effectiveMass += softnessOverDt;
if (effectiveMass != 0)
{
effectiveMass = 1.0f / effectiveMass;
}
bias = -(l % (p2 - p1)).Length() * BiasFactor * (1.0f / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * AppliedImpulse * jacobian[0];
body1.angularVelocity += JVector.Transform(AppliedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * AppliedImpulse * jacobian[2];
body2.angularVelocity += JVector.Transform(AppliedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
public static JVector ComputeNormal(JVector p0, JVector p1, JVector p2, WindingTypes winding,
bool shouldNormalize = true)
{
var v0 = JVector.Subtract(p1, p0);
var v1 = JVector.Subtract(p2, p0);
// This calculation is the same as the one used in a constructor below, but due to using JVector vs. vec3,
// it's easier to just duplicate the code.
var v = JVector.Cross(v0, v1) * (winding == WindingTypes.Clockwise ? 1 : -1);
return(shouldNormalize ? JVector.Normalize(v) : v);
}
public PointOnLine(RigidBody body, JVector localAnchor, JVector lineDirection) : base(body, null)
{
if (lineDirection.LengthSquared() == 0.0f)
{
throw new ArgumentException("Line direction can't be zero", nameof(lineDirection));
}
localAnchor1 = localAnchor;
anchor = body.position + JVector.Transform(localAnchor, body.orientation);
lineNormal = lineDirection;
lineNormal = JVector.Normalize(lineNormal);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(float timestep)
{
JVector dp;
JVector.Subtract(ref body2.position, ref body1.position, out dp);
float deltaLength = dp.Length() - distance;
if (behavior == DistanceBehavior.LimitMaximumDistance && deltaLength <= 0.0f)
{
skipConstraint = true;
}
else if (behavior == DistanceBehavior.LimitMinimumDistance && deltaLength >= 0.0f)
{
skipConstraint = true;
}
else
{
skipConstraint = false;
JVector n = dp;
if (n.LengthSquared() != 0.0f)
{
n.Normalize();
}
jacobian[0] = -1.0f * n;
//jacobian[1] = -1.0f * (r1 % n);
jacobian[1] = 1.0f * n;
//jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass;
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = 1.0f / effectiveMass;
bias = deltaLength * biasFactor * (1.0f / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[1];
}
}
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(float timestep)
{
JVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
JVector.Transform(ref localAnchor2, ref body2.orientation, out r2);
JVector p1, p2, dp;
JVector.Add(ref body1.position, ref r1, out p1);
JVector.Add(ref body2.position, ref r2, out p2);
JVector.Subtract(ref p2, ref p1, out dp);
float deltaLength = dp.Length();
JVector n = p2 - p1;
if (n.LengthSquared() != 0.0f)
{
n.Normalize();
}
jacobian[0] = -1.0f * n;
jacobian[1] = -1.0f * (r1 % n);
jacobian[2] = 1.0f * n;
jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass
+ JVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ JVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = 1.0f / effectiveMass;
bias = deltaLength * biasFactor * (1.0f / timestep);
// CUSTOM: Modified to use the IsStatic property (plus the condition below).
if (!body1.IsStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.IsStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2];
body2.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
public LimitedHingeJoint(World world, RigidBody body1, RigidBody body2, JVector position, JVector hingeAxis, float hingeFwdAngle, float hingeBckAngle) : base(world)
{
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= 0.5f;
var pos1 = position;
JVector.Add(pos1, hingeAxis, out pos1);
var pos2 = position;
JVector.Subtract(pos2, hingeAxis, out pos2);
worldPointConstraint[0] = new PointOnPoint(body1, body2, pos1);
worldPointConstraint[1] = new PointOnPoint(body1, body2, pos2);
hingeAxis = JVector.Normalize(hingeAxis);
var perpDir = JVector.Up;
if (JVector.Dot(perpDir, hingeAxis) > 0.1f)
{
perpDir = JVector.Right;
}
var sideAxis = JVector.Cross(hingeAxis, perpDir);
perpDir = JVector.Cross(sideAxis, hingeAxis);
perpDir = JVector.Normalize(perpDir);
float len = 10.0f * 3;
var hingeRelAnchorPos0 = perpDir * len;
float angleToMiddle = 0.5f * (hingeFwdAngle - hingeBckAngle);
var hingeRelAnchorPos1 = JVector.Transform(hingeRelAnchorPos0, JMatrix.CreateFromAxisAngle(hingeAxis, -angleToMiddle / 360.0f * 2.0f * JMath.Pi));
float hingeHalfAngle = 0.5f * (hingeFwdAngle + hingeBckAngle);
float allowedDistance = len * 2.0f * (float)System.Math.Sin(hingeHalfAngle * 0.5f / 360.0f * 2.0f * JMath.Pi);
var hingePos = body1.Position;
var relPos0c = hingePos + hingeRelAnchorPos0;
var relPos1c = hingePos + hingeRelAnchorPos1;
DistanceConstraint = new PointPointDistance(body1, body2, relPos0c, relPos1c)
{
Distance = allowedDistance,
Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance
};
}
/// <summary>
///
/// </summary>
/// <param name="rayOrigin"></param>
/// <param name="rayDelta"></param>
/// <returns></returns>
public override int Prepare(ref JVector rayOrigin, ref JVector rayDelta)
{
potentialTriangles.Clear();
#region Expand Spherical
JVector expDelta;
JVector.Normalize(ref rayDelta, out expDelta);
expDelta = rayDelta + expDelta * sphericalExpansion;
#endregion
octree.GetTrianglesIntersectingRay(potentialTriangles, rayOrigin, expDelta);
return(potentialTriangles.Count);
}
/// <summary>
///
/// </summary>
/// <param name="rayOrigin"></param>
/// <param name="rayDelta"></param>
/// <returns></returns>
public override int Prepare(ref JVector rayOrigin, ref JVector rayDelta)
{
JBBox box = JBBox.SmallBox;
#region RayEnd + Expand Spherical
JVector rayEnd;
JVector.Normalize(ref rayDelta, out rayEnd);
rayEnd = rayOrigin + rayDelta + rayEnd * sphericalExpansion;
#endregion
box.AddPoint(ref rayOrigin);
box.AddPoint(ref rayEnd);
return(this.Prepare(ref box));
}
public override void PrepareForIteration(float timestep)
{
JVector.Transform(localAnchor1, body1.orientation, out r1);
JVector.Transform(localAnchor2, body2.orientation, out r2);
JVector.Add(body1.position, r1, out var p1);
JVector.Add(body2.position, r2, out var p2);
JVector.Subtract(p2, p1, out var dp);
float deltaLength = dp.Length();
var n = p2 - p1;
if (n.LengthSquared() != 0.0f)
{
n = JVector.Normalize(n);
}
jacobian[0] = -1.0f * n;
jacobian[1] = -1.0f * (r1 % n);
jacobian[2] = 1.0f * n;
jacobian[3] = r2 % n;
effectiveMass = body1.inverseMass + body2.inverseMass
+ (JVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1])
+ (JVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3]);
softnessOverDt = Softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = 1.0f / effectiveMass;
bias = deltaLength * BiasFactor * (1.0f / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * AppliedImpulse * jacobian[0];
body1.angularVelocity += JVector.Transform(AppliedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * AppliedImpulse * jacobian[2];
body2.angularVelocity += JVector.Transform(AppliedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
public override void PrepareForIteration(float timestep)
{
JVector.Subtract(body2.position, body1.position, out var dp);
float deltaLength = dp.Length() - Distance;
if (Behavior == DistanceBehavior.LimitMaximumDistance && deltaLength <= 0.0f)
{
skipConstraint = true;
}
else if (Behavior == DistanceBehavior.LimitMinimumDistance && deltaLength >= 0.0f)
{
skipConstraint = true;
}
else
{
skipConstraint = false;
var n = dp;
if (n.LengthSquared() != 0.0f)
{
n = JVector.Normalize(n);
}
jacobian[0] = -1.0f * n;
jacobian[1] = 1.0f * n;
effectiveMass = body1.inverseMass + body2.inverseMass;
softnessOverDt = Softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = 1.0f / effectiveMass;
bias = deltaLength * BiasFactor * (1.0f / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * AppliedImpulse * jacobian[0];
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * AppliedImpulse * jacobian[1];
}
}
}
/// <summary>
/// Discrete Circle vs Circle test. Very fast. Generates contact info.
/// NOTE: check distance for collisions. If negative then a collision has occurred.
/// This is done to remove all branches from this test and leave it to the user to decide when to branch.
/// </summary>
public static void CircleCircleTest(JVector centerA, float radiusA, JVector centerB, float radiusB, out JVector pointA, out JVector pointB, out JVector normal, out float distance)
{
// ||A-B|| - (r1+r2) < 0
float d = JVector.DistanceSquared(centerA, centerB);
float r = (radiusA + radiusB);
r *= r;
distance = d - r;
normal = (centerA - centerB) / d;
normal.Normalize();
// calculate closest 2 points
pointA = JVector.Negate(normal) * radiusA + centerA;
pointB = normal * radiusB + centerB;
}
// This is primarily used for tracking relative rotation on moving platforms.
public static float ComputeYaw(this JMatrix matrix)
{
// See https://stackoverflow.com/a/4341489/7281613.
JVector t = JVector.Transform(JVector.Left, matrix);
JVector f = t - JVector.Dot(t, JVector.Up) * JVector.Up;
f.Normalize();
float angle = (float)Math.Acos(JVector.Dot(JVector.Left, f));
float d = JVector.Dot(JVector.Up, JVector.Cross(JVector.Left, f));
if (d < 0)
{
angle = Constants.TwoPi - angle;
}
return(angle);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The simulation timestep</param>
public override void PrepareForIteration(float timestep)
{
JVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
JVector p1, dp;
JVector.Add(ref body1.position, ref r1, out p1);
JVector.Subtract(ref p1, ref anchor, out dp);
JVector l = lineNormal;
JVector t = (p1 - anchor) % l;
if (t.LengthSquared() != 0.0f)
{
t.Normalize();
}
t = t % l;
jacobian[0] = t;
jacobian[1] = r1 % t;
effectiveMass = body1.inverseMass
+ JVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
if (effectiveMass != 0)
{
effectiveMass = 1.0f / effectiveMass;
}
bias = -(l % (p1 - anchor)).Length() * biasFactor * (1.0f / timestep);
// CUSTOM: Modified to use the IsStatic property.
if (!body1.IsStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
}
public override void PrepareForIteration(float timestep)
{
var delta = Target - Body1.Position;
var length = delta.Length();
if (length > 1f * Velocity)
{
if (Velocity > targetVelocity.Length())
{
targetVelocity = JVector.Normalize(delta) * (targetVelocity.Length() + Velocity * timestep);
}
else
{
targetVelocity = JVector.Normalize(delta) * Velocity;
}
}
else
{
targetVelocity = JVector.Normalize(delta) * (float)Math.Sqrt(length);
}
}
internal static bool CircleCapsuleTest(JVector centerA, float radiusA, JVector centerB, JVector axis, float length, float radiusB, out JVector pointA, out JVector pointB, out JVector normal, out float distance)
{
// get capsule endpoints
var p0 = centerB - axis * (length * 0.5f);
var p1 = centerB + axis * (length * 0.5f);
// get vector from endpoint to circle
var D = centerA - p0;
// project vector onto axis and clamp
var d = JVector.Dot(D, axis);
d = JMath.Clamp(d, 0, length);
// get point on axis
var R = p0 + axis * d;
// distance
var b = Math.Abs((centerA - R).Length());
normal = (centerA - R) / b;
// calculate closest 2 points
var RH = JVector.Normalize(centerA - R);
pointA = JVector.Negate(RH) * radiusA + centerA;
pointB = RH * radiusB + R;
normal.Negate();
distance = b - (radiusA + radiusB);
//
if (b < radiusA + radiusB)
{
return(true);
}
return(false);
}
