/// <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)); }