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