/// <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.
System.Numerics.Quaternion conjugate;
QuaternionEx.Conjugate(ref transform.Orientation, out conjugate);
Ray localRay;
Vector3Ex.Subtract(ref ray.Position, ref transform.Position, out localRay.Position);
QuaternionEx.Transform(ref localRay.Position, ref conjugate, out localRay.Position);
QuaternionEx.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 System.Numerics.Vector3(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared();
if (normalLengthSquared > 1e-9f)
Vector3Ex.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
else
hit.Normal = new System.Numerics.Vector3();
//Pull the hit into world space.
QuaternionEx.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.
System.Numerics.Vector2 planeDirection = new System.Numerics.Vector2(localRay.Direction.X, localRay.Direction.Z);
float planeDirectionLengthSquared = planeDirection.LengthSquared();
if (planeDirectionLengthSquared < Toolbox.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;
}
System.Numerics.Vector2 planeOrigin = new System.Numerics.Vector2(localRay.Position.X, localRay.Position.Z);
float dot;
Vector2Ex.Dot(ref planeDirection, ref planeOrigin, out dot);
float closestToCenterT = -dot / planeDirectionLengthSquared;
System.Numerics.Vector2 closestPoint;
Vector2Ex.Multiply(ref planeDirection, closestToCenterT, out closestPoint);
Vector2Ex.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.
Vector3Ex.Multiply(ref localRay.Direction, hit.T, out hit.Location);
Vector3Ex.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 System.Numerics.Vector3(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared();
if (normalLengthSquared > 1e-9f)
Vector3Ex.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
else
hit.Normal = new System.Numerics.Vector3();
//Pull the hit into world space.
QuaternionEx.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 System.Numerics.Vector3(0, halfLength, 0);
if (Toolbox.RayCastSphere(ref localRay, ref spherePosition, collisionMargin, maximumLength, out hit))
{
//Pull the hit into world space.
QuaternionEx.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 System.Numerics.Vector3(0, -halfLength, 0);
if (Toolbox.RayCastSphere(ref localRay, ref spherePosition, collisionMargin, maximumLength, out hit))
{
//Pull the hit into world space.
QuaternionEx.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>
/// 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.
System.Numerics.Quaternion conjugate;
QuaternionEx.Conjugate(ref transform.Orientation, out conjugate);
Ray localRay;
Vector3Ex.Subtract(ref ray.Position, ref transform.Position, out localRay.Position);
QuaternionEx.Transform(ref localRay.Position, ref conjugate, out localRay.Position);
QuaternionEx.Transform(ref ray.Direction, ref conjugate, out localRay.Direction);
//Check for containment.
if (localRay.Position.Y >= -halfHeight && localRay.Position.Y <= halfHeight && localRay.Position.X * localRay.Position.X + localRay.Position.Z * localRay.Position.Z <= radius * radius)
{
//It's inside!
hit.T = 0;
hit.Location = localRay.Position;
hit.Normal = new System.Numerics.Vector3(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared();
if (normalLengthSquared > 1e-9f)
{
Vector3Ex.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
}
else
{
hit.Normal = new System.Numerics.Vector3();
}
//Pull the hit into world space.
QuaternionEx.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.
System.Numerics.Vector2 planeDirection = new System.Numerics.Vector2(localRay.Direction.X, localRay.Direction.Z);
float planeDirectionLengthSquared = planeDirection.LengthSquared();
if (planeDirectionLengthSquared < Toolbox.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 > halfHeight)
{
goto upperTest;
}
if (localRay.Position.Y < -halfHeight)
{
goto lowerTest;
}
hit = new RayHit();
return(false);
}
System.Numerics.Vector2 planeOrigin = new System.Numerics.Vector2(localRay.Position.X, localRay.Position.Z);
float dot;
Vector2Ex.Dot(ref planeDirection, ref planeOrigin, out dot);
float closestToCenterT = -dot / planeDirectionLengthSquared;
System.Numerics.Vector2 closestPoint;
Vector2Ex.Multiply(ref planeDirection, closestToCenterT, out closestPoint);
Vector2Ex.Add(ref planeOrigin, ref closestPoint, out closestPoint);
//How close does the ray come to the circle?
float squaredDistance = closestPoint.LengthSquared();
if (squaredDistance > radius * radius)
{
//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 = radius * (float)Math.Sqrt(1 - squaredDistance / (radius * radius));
float tOffset = backwardsDistance / (float)Math.Sqrt(planeDirectionLengthSquared);
hit.T = closestToCenterT - tOffset;
//Compute the impact point on the infinite cylinder in 3d local space.
Vector3Ex.Multiply(ref localRay.Direction, hit.T, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref localRay.Position, out hit.Location);
//Is it intersecting the cylindrical portion of the capsule?
if (hit.Location.Y <= halfHeight && hit.Location.Y >= -halfHeight && hit.T < maximumLength)
{
//Yup!
hit.Normal = new System.Numerics.Vector3(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared();
if (normalLengthSquared > 1e-9f)
{
Vector3Ex.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
}
else
{
hit.Normal = new System.Numerics.Vector3();
}
//Pull the hit into world space.
QuaternionEx.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 < halfHeight)
{
goto lowerTest;
}
upperTest:
//Nope! It may be intersecting the ends of the cylinder though.
//We're above the cylinder, so cast a ray against the upper cap.
if (localRay.Direction.Y > -1e-9)
{
//Can't hit the upper cap if the ray isn't pointing down.
hit = new RayHit();
return(false);
}
float t = (halfHeight - localRay.Position.Y) / localRay.Direction.Y;
System.Numerics.Vector3 planeIntersection;
Vector3Ex.Multiply(ref localRay.Direction, t, out planeIntersection);
Vector3Ex.Add(ref localRay.Position, ref planeIntersection, out planeIntersection);
if (planeIntersection.X * planeIntersection.X + planeIntersection.Z * planeIntersection.Z < radius * radius + 1e-9 && t < maximumLength)
{
//Pull the hit into world space.
QuaternionEx.Transform(ref Toolbox.UpVector, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref planeIntersection, ref transform, out hit.Location);
hit.T = t;
return(true);
}
//No intersection! We can't be hitting the other sphere, so it's over!
hit = new RayHit();
return(false);
lowerTest:
//Is it intersecting the bottom cap?
if (localRay.Direction.Y < 1e-9)
{
//Can't hit the bottom cap if the ray isn't pointing up.
hit = new RayHit();
return(false);
}
t = (-halfHeight - localRay.Position.Y) / localRay.Direction.Y;
Vector3Ex.Multiply(ref localRay.Direction, t, out planeIntersection);
Vector3Ex.Add(ref localRay.Position, ref planeIntersection, out planeIntersection);
if (planeIntersection.X * planeIntersection.X + planeIntersection.Z * planeIntersection.Z < radius * radius + 1e-9 && t < maximumLength)
{
//Pull the hit into world space.
QuaternionEx.Transform(ref Toolbox.DownVector, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref planeIntersection, ref transform, out hit.Location);
hit.T = t;
return(true);
}
//No intersection! We can't be hitting the other sphere, so it's over!
hit = new RayHit();
return(false);
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
Matrix3x3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA);
Matrix3x3.Transform(ref localAxisB, ref connectionB.orientationMatrix, out worldAxisB);
Matrix3x3.Transform(ref localConstrainedAxis1, ref connectionA.orientationMatrix, out worldConstrainedAxis1);
Matrix3x3.Transform(ref localConstrainedAxis2, ref connectionA.orientationMatrix, out worldConstrainedAxis2);
System.Numerics.Vector3 error;
Vector3Ex.Cross(ref worldAxisA, ref worldAxisB, out error);
Vector3Ex.Dot(ref error, ref worldConstrainedAxis1, out this.error.X);
Vector3Ex.Dot(ref error, ref worldConstrainedAxis2, out this.error.Y);
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);
errorReduction = -errorReduction;
biasVelocity.X = errorReduction * this.error.X;
biasVelocity.Y = errorReduction * this.error.Y;
//Ensure that the corrective velocity doesn't exceed the max.
float length = biasVelocity.LengthSquared();
if (length > maxCorrectiveVelocitySquared)
{
float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length);
biasVelocity.X *= multiplier;
biasVelocity.Y *= multiplier;
}
System.Numerics.Vector3 axis1I, axis2I;
if (connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3x3 inertiaTensorSum;
Matrix3x3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out inertiaTensorSum);
Matrix3x3.Transform(ref worldConstrainedAxis1, ref inertiaTensorSum, out axis1I);
Matrix3x3.Transform(ref worldConstrainedAxis2, ref inertiaTensorSum, out axis2I);
}
else if (connectionA.isDynamic && !connectionB.isDynamic)
{
Matrix3x3.Transform(ref worldConstrainedAxis1, ref connectionA.inertiaTensorInverse, out axis1I);
Matrix3x3.Transform(ref worldConstrainedAxis2, ref connectionA.inertiaTensorInverse, out axis2I);
}
else if (!connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3x3.Transform(ref worldConstrainedAxis1, ref connectionB.inertiaTensorInverse, out axis1I);
Matrix3x3.Transform(ref worldConstrainedAxis2, ref connectionB.inertiaTensorInverse, out axis2I);
}
else
{
throw new InvalidOperationException("Cannot constrain two kinematic bodies.");
}
Vector3Ex.Dot(ref axis1I, ref worldConstrainedAxis1, out effectiveMassMatrix.M11);
Vector3Ex.Dot(ref axis1I, ref worldConstrainedAxis2, out effectiveMassMatrix.M12);
Vector3Ex.Dot(ref axis2I, ref worldConstrainedAxis1, out effectiveMassMatrix.M21);
Vector3Ex.Dot(ref axis2I, ref worldConstrainedAxis2, out effectiveMassMatrix.M22);
effectiveMassMatrix.M11 += softness;
effectiveMassMatrix.M22 += softness;
Matrix2x2.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
Matrix2x2.Negate(ref effectiveMassMatrix, out effectiveMassMatrix);
}
/// <summary>
/// Computes a solution to the constraint.
/// </summary>
/// <returns>Impulse magnitude computed by the iteration.</returns>
public override float SolveIteration()
{
System.Numerics.Vector2 relativeVelocity = RelativeVelocity;
Vector2Ex.Add(ref relativeVelocity, ref positionCorrectionBias, out relativeVelocity);
//Create the full velocity change, and convert it to an impulse in constraint space.
System.Numerics.Vector2 lambda;
Vector2Ex.Subtract(ref targetVelocity, ref relativeVelocity, out lambda);
Matrix2x2.Transform(ref lambda, ref massMatrix, out lambda);
//Add and clamp the impulse.
System.Numerics.Vector2 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, maxForce);
accumulatedImpulse.Y = 0;
}
else
{
Vector2Ex.Add(ref lambda, ref accumulatedImpulse, out accumulatedImpulse);
float length = accumulatedImpulse.LengthSquared();
if (length > maxForce * maxForce)
{
Vector2Ex.Multiply(ref accumulatedImpulse, maxForce / (float)Math.Sqrt(length), out accumulatedImpulse);
}
}
Vector2Ex.Subtract(ref accumulatedImpulse, ref previousAccumulatedImpulse, out lambda);
//Use the jacobians to put the impulse into world space.
#if !WINDOWS
System.Numerics.Vector3 impulse = new System.Numerics.Vector3();
System.Numerics.Vector3 torque = new System.Numerics.Vector3();
#else
System.Numerics.Vector3 impulse;
System.Numerics.Vector3 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)
{
Vector3Ex.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));
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//Transform local axes into world space
Matrix3x3.Transform(ref localRestrictedAxis1, ref connectionA.orientationMatrix, out worldRestrictedAxis1);
Matrix3x3.Transform(ref localRestrictedAxis2, ref connectionA.orientationMatrix, out worldRestrictedAxis2);
Matrix3x3.Transform(ref localAxisAnchor, ref connectionA.orientationMatrix, out worldLineAnchor);
Vector3Ex.Add(ref worldLineAnchor, ref connectionA.position, out worldLineAnchor);
Matrix3x3.Transform(ref localLineDirection, ref connectionA.orientationMatrix, out worldLineDirection);
//Transform local
Matrix3x3.Transform(ref localPoint, ref connectionB.orientationMatrix, out rB);
Vector3Ex.Add(ref rB, ref connectionB.position, out worldPoint);
//Find the point on the line closest to the world point.
System.Numerics.Vector3 offset;
Vector3Ex.Subtract(ref worldPoint, ref worldLineAnchor, out offset);
float distanceAlongAxis;
Vector3Ex.Dot(ref offset, ref worldLineDirection, out distanceAlongAxis);
System.Numerics.Vector3 worldNearPoint;
Vector3Ex.Multiply(ref worldLineDirection, distanceAlongAxis, out offset);
Vector3Ex.Add(ref worldLineAnchor, ref offset, out worldNearPoint);
Vector3Ex.Subtract(ref worldNearPoint, ref connectionA.position, out rA);
//Error
System.Numerics.Vector3 error3D;
Vector3Ex.Subtract(ref worldPoint, ref worldNearPoint, out error3D);
Vector3Ex.Dot(ref error3D, ref worldRestrictedAxis1, out error.X);
Vector3Ex.Dot(ref error3D, ref worldRestrictedAxis2, out error.Y);
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);
float bias = -errorReduction;
biasVelocity.X = bias * error.X;
biasVelocity.Y = bias * error.Y;
//Ensure that the corrective velocity doesn't exceed the max.
float length = biasVelocity.LengthSquared();
if (length > maxCorrectiveVelocitySquared)
{
float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length);
biasVelocity.X *= multiplier;
biasVelocity.Y *= multiplier;
}
//Set up the jacobians
Vector3Ex.Cross(ref rA, ref worldRestrictedAxis1, out angularA1);
Vector3Ex.Cross(ref worldRestrictedAxis1, ref rB, out angularB1);
Vector3Ex.Cross(ref rA, ref worldRestrictedAxis2, out angularA2);
Vector3Ex.Cross(ref worldRestrictedAxis2, ref rB, out angularB2);
float m11 = 0, m22 = 0, m1221 = 0;
float inverseMass;
System.Numerics.Vector3 intermediate;
//Compute the effective mass matrix.
if (connectionA.isDynamic)
{
inverseMass = connectionA.inverseMass;
Matrix3x3.Transform(ref angularA1, ref connectionA.inertiaTensorInverse, out intermediate);
Vector3Ex.Dot(ref intermediate, ref angularA1, out m11);
m11 += inverseMass;
Vector3Ex.Dot(ref intermediate, ref angularA2, out m1221);
Matrix3x3.Transform(ref angularA2, ref connectionA.inertiaTensorInverse, out intermediate);
Vector3Ex.Dot(ref intermediate, ref angularA2, out m22);
m22 += inverseMass;
}
#region Mass System.Numerics.Matrix4x4 B
if (connectionB.isDynamic)
{
float extra;
inverseMass = connectionB.inverseMass;
Matrix3x3.Transform(ref angularB1, ref connectionB.inertiaTensorInverse, out intermediate);
Vector3Ex.Dot(ref intermediate, ref angularB1, out extra);
m11 += inverseMass + extra;
Vector3Ex.Dot(ref intermediate, ref angularB2, out extra);
m1221 += extra;
Matrix3x3.Transform(ref angularB2, ref connectionB.inertiaTensorInverse, out intermediate);
Vector3Ex.Dot(ref intermediate, ref angularB2, out extra);
m22 += inverseMass + extra;
}
#endregion
negativeEffectiveMassMatrix.M11 = m11 + softness;
negativeEffectiveMassMatrix.M12 = m1221;
negativeEffectiveMassMatrix.M21 = m1221;
negativeEffectiveMassMatrix.M22 = m22 + softness;
Matrix2x2.Invert(ref negativeEffectiveMassMatrix, out negativeEffectiveMassMatrix);
Matrix2x2.Negate(ref negativeEffectiveMassMatrix, out negativeEffectiveMassMatrix);
}
/// <summary>
/// updates the particle. Returns true when the particle is no longer alive
/// </summary>
/// <param name="emitterConfig">Emitter config.</param>
public bool Update(ParticleEmitterConfig emitterConfig, ref ParticleCollisionConfig collisionConfig,
System.Numerics.Vector2 rootPosition)
{
// PART 1: reduce the life span of the particle
_timeToLive -= Time.DeltaTime;
// if the current particle is alive then update it
if (_timeToLive > 0)
{
// only update the particle position if it has not collided. If it has, physics takes over
if (!_collided)
{
// if maxRadius is greater than 0 then the particles are going to spin otherwise they are affected by speed and gravity
if (emitterConfig.EmitterType == ParticleEmitterType.Radial)
{
// PART 2: update the angle of the particle from the radius. This is only done if the particles are rotating
_angle += _degreesPerSecond * Time.DeltaTime;
_radius += _radiusDelta * Time.DeltaTime;
System.Numerics.Vector2 tmp;
tmp.X = -Mathf.Cos(_angle) * _radius;
tmp.Y = -Mathf.Sin(_angle) * _radius;
_velocity = tmp - position;
position = tmp;
}
else
{
System.Numerics.Vector2 tmp, radial, tangential;
radial = System.Numerics.Vector2.Zero;
if (position.X != 0 || position.Y != 0)
{
radial = System.Numerics.Vector2.Normalize(position);
}
tangential = radial;
radial = radial * _radialAcceleration;
var newy = tangential.X;
tangential.X = -tangential.Y;
tangential.Y = newy;
tangential = tangential * _tangentialAcceleration;
tmp = radial + tangential + emitterConfig.Gravity;
tmp = tmp * Time.DeltaTime;
_direction = _direction + tmp;
tmp = _direction * Time.DeltaTime;
_velocity = tmp / Time.DeltaTime;
position = position + tmp;
}
}
// update the particles color. we do the lerp from finish-to-start because timeToLive counts from particleLifespan to 0
var t = (_particleLifetime - _timeToLive) / _particleLifetime;
ColorExt.Lerp(ref _startColor, ref _finishColor, out color, t);
// update the particle size
particleSize += _particleSizeDelta * Time.DeltaTime;
particleSize = MathHelper.Max(0, particleSize);
// update the rotation of the particle
rotation += _rotationDelta * Time.DeltaTime;
if (collisionConfig.Enabled)
{
// if we already collided we have to handle the collision response
if (_collided)
{
// handle after collision movement. we need to track velocity for this
_velocity += collisionConfig.Gravity * Time.DeltaTime;
position += _velocity * Time.DeltaTime;
// if we move too slow we die
if (_velocity.LengthSquared() < collisionConfig.MinKillSpeedSquared)
{
return(true);
}
}
// should we use our spawnPosition as a reference or the parent Transforms position?
var pos = emitterConfig.SimulateInWorldSpace ? spawnPosition : rootPosition;
_circleCollisionShape.RecalculateBounds(particleSize * 0.5f * collisionConfig.RadiusScale,
pos + position);
var neighbors = Physics.BoxcastBroadphase(ref _circleCollisionShape.bounds,
collisionConfig.CollidesWithLayers);
foreach (var neighbor in neighbors)
{
CollisionResult result;
if (_circleCollisionShape.CollidesWithShape(neighbor.Shape, out result))
{
// handle the overlap
position -= result.MinimumTranslationVector;
CalculateCollisionResponseVelocity(collisionConfig.Friction, collisionConfig.Elasticity,
ref result.MinimumTranslationVector);
// handle collision config props
_timeToLive -= _timeToLive * collisionConfig.LifetimeLoss;
_collided = true;
}
}
}
}
else
{
// timeToLive expired. were done
return(true);
}
return(false);
}
//Extracted from Box2D
/// <summary>
/// Returns the convex hull from the given vertices.
/// </summary>
/// <param name="vertices">The vertices.</param>
public static Vertices GetConvexHull(Vertices vertices)
{
if (vertices.Count <= 3)
{
return(vertices);
}
// Find the right most point on the hull
int i0 = 0;
float x0 = vertices[0].X;
for (int i = 1; i < vertices.Count; ++i)
{
float x = vertices[i].X;
if (x > x0 || (x == x0 && vertices[i].Y < vertices[i0].Y))
{
i0 = i;
x0 = x;
}
}
int[] hull = new int[vertices.Count];
int m = 0;
int ih = i0;
for (;;)
{
hull[m] = ih;
int ie = 0;
for (int j = 1; j < vertices.Count; ++j)
{
if (ie == ih)
{
ie = j;
continue;
}
System.Numerics.Vector2 r = vertices[ie] - vertices[hull[m]];
System.Numerics.Vector2 v = vertices[j] - vertices[hull[m]];
float c = MathUtils.Cross(ref r, ref v);
if (c < 0.0f)
{
ie = j;
}
// Collinearity check
if (c == 0.0f && v.LengthSquared() > r.LengthSquared())
{
ie = j;
}
}
++m;
ih = ie;
if (ie == i0)
{
break;
}
}
Vertices result = new Vertices(m);
// Copy vertices.
for (int i = 0; i < m; ++i)
{
result.Add(vertices[hull[i]]);
}
return(result);
}
/// <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()
{
System.Numerics.Vector2 lambda = RelativeVelocity;
//Convert to impulse
//Matrix2x2.Transform(ref lambda, ref velocityToImpulse, out lambda);
float x = lambda.X;
lambda.X = x * velocityToImpulse.M11 + lambda.Y * velocityToImpulse.M21;
lambda.Y = x * velocityToImpulse.M12 + lambda.Y * velocityToImpulse.M22;
//Accumulate and clamp
System.Numerics.Vector2 previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse.X += lambda.X;
accumulatedImpulse.Y += lambda.Y;
float length = accumulatedImpulse.LengthSquared();
float maximumFrictionForce = 0;
for (int i = 0; i < contactCount; i++)
{
maximumFrictionForce += contactManifoldConstraint.penetrationConstraints.Elements[i].accumulatedImpulse;
}
maximumFrictionForce *= friction;
if (length > maximumFrictionForce * maximumFrictionForce)
{
length = maximumFrictionForce / (float)Math.Sqrt(length);
accumulatedImpulse.X *= length;
accumulatedImpulse.Y *= length;
}
lambda.X = accumulatedImpulse.X - previousAccumulatedImpulse.X;
lambda.Y = accumulatedImpulse.Y - previousAccumulatedImpulse.Y;
//Single Axis clamp
//float maximumFrictionForce = 0;
//for (int i = 0; i < contactCount; i++)
//{
// maximumFrictionForce += pair.contacts[i].penetrationConstraint.accumulatedImpulse;
//}
//maximumFrictionForce *= friction;
//float previousAccumulatedImpulse = accumulatedImpulse.X;
//accumulatedImpulse.X = MathHelper.Clamp(accumulatedImpulse.X + lambda.X, -maximumFrictionForce, maximumFrictionForce);
//lambda.X = accumulatedImpulse.X - previousAccumulatedImpulse;
//previousAccumulatedImpulse = accumulatedImpulse.Y;
//accumulatedImpulse.Y = MathHelper.Clamp(accumulatedImpulse.Y + lambda.Y, -maximumFrictionForce, maximumFrictionForce);
//lambda.Y = accumulatedImpulse.Y - previousAccumulatedImpulse;
//Apply impulse
#if !WINDOWS
System.Numerics.Vector3 linear = new System.Numerics.Vector3();
System.Numerics.Vector3 angular = new System.Numerics.Vector3();
#else
System.Numerics.Vector3 linear, angular;
#endif
//Matrix2x3.Transform(ref lambda, ref linearA, out linear);
linear.X = lambda.X * linearA.M11 + lambda.Y * linearA.M21;
linear.Y = lambda.X * linearA.M12 + lambda.Y * linearA.M22;
linear.Z = lambda.X * linearA.M13 + lambda.Y * linearA.M23;
if (entityADynamic)
{
//Matrix2x3.Transform(ref lambda, ref angularA, out angular);
angular.X = lambda.X * angularA.M11 + lambda.Y * angularA.M21;
angular.Y = lambda.X * angularA.M12 + lambda.Y * angularA.M22;
angular.Z = lambda.X * angularA.M13 + lambda.Y * angularA.M23;
entityA.ApplyLinearImpulse(ref linear);
entityA.ApplyAngularImpulse(ref angular);
}
if (entityBDynamic)
{
linear.X = -linear.X;
linear.Y = -linear.Y;
linear.Z = -linear.Z;
//Matrix2x3.Transform(ref lambda, ref angularB, out angular);
angular.X = lambda.X * angularB.M11 + lambda.Y * angularB.M21;
angular.Y = lambda.X * angularB.M12 + lambda.Y * angularB.M22;
angular.Z = lambda.X * angularB.M13 + lambda.Y * angularB.M23;
entityB.ApplyLinearImpulse(ref linear);
entityB.ApplyAngularImpulse(ref angular);
}
return(Math.Abs(lambda.X) + Math.Abs(lambda.Y));
}
