rigidbody, forces and collision testing
This commit is contained in:
maxwes08
123
PhysicsEngine/Physics/Bodies/Body.cs
Normal file
123
PhysicsEngine/Physics/Bodies/Body.cs
Normal file
@@ -0,0 +1,123 @@
|
||||
using System;
|
||||
using System.Numerics;
|
||||
using Raylib_cs;
|
||||
|
||||
namespace PhysicsEngine
|
||||
{
|
||||
public class Body
|
||||
{
|
||||
public Vector2 Position;
|
||||
public Vector2 Velocity;
|
||||
public Vector2 ForceAccumulator;
|
||||
public Color BaseColor = Color.Blue;
|
||||
public float Rotation;
|
||||
public float AngularVelocity;
|
||||
public float TorqueAccumulator;
|
||||
public float Mass;
|
||||
public float InverseMass => Mass > 0.0f ? 1.0f / Mass : 0.0f;
|
||||
public float MomentOfInertia;
|
||||
public float InverseInertia => MomentOfInertia > 0.0f ? 1.0f / MomentOfInertia : 0.0f;
|
||||
public float Restitution = 0.5f;
|
||||
public float Friction = 0.2f;
|
||||
public float DragCoefficient = 0.0f; // Drag per m^2 (ignored if 0.0)
|
||||
public bool IsStatic => Mass == 0.0f;
|
||||
|
||||
public void ApplyForce(Vector2 force)
|
||||
{
|
||||
if (IsStatic) return;
|
||||
ForceAccumulator += force;
|
||||
}
|
||||
|
||||
public void ApplyTorque(float torque)
|
||||
{
|
||||
if (IsStatic) return;
|
||||
TorqueAccumulator += torque;
|
||||
}
|
||||
|
||||
// --- Impulse Methods ---
|
||||
|
||||
public void ApplyImpulse(Vector2 impulse)
|
||||
{
|
||||
if (IsStatic) return;
|
||||
Velocity += impulse * InverseMass;
|
||||
}
|
||||
|
||||
public void ApplyImpulseAtOffset(Vector2 impulse, Vector2 localOffset)
|
||||
{
|
||||
if (IsStatic) return;
|
||||
|
||||
// Direct linear velocity change
|
||||
Velocity += impulse * InverseMass;
|
||||
|
||||
// Rotate local offset into world space based on body rotation
|
||||
float cos = MathF.Cos(Rotation);
|
||||
float sin = MathF.Sin(Rotation);
|
||||
Vector2 worldOffset = new Vector2(
|
||||
localOffset.X * cos - localOffset.Y * sin,
|
||||
localOffset.X * sin + localOffset.Y * cos
|
||||
);
|
||||
|
||||
// 2D Cross product for torque: r x J
|
||||
float torque = worldOffset.X * impulse.Y - worldOffset.Y * impulse.X;
|
||||
AngularVelocity += torque * InverseInertia;
|
||||
}
|
||||
|
||||
public void ApplyImpulseAtWorldPosition(Vector2 impulse, Vector2 worldPosition)
|
||||
{
|
||||
if (IsStatic) return;
|
||||
|
||||
// Direct linear velocity change
|
||||
Velocity += impulse * InverseMass;
|
||||
|
||||
// Offset from center of mass in world space
|
||||
Vector2 worldOffset = worldPosition - Position;
|
||||
|
||||
// 2D Cross product for torque: r x J
|
||||
float torque = worldOffset.X * impulse.Y - worldOffset.Y * impulse.X;
|
||||
AngularVelocity += torque * InverseInertia;
|
||||
}
|
||||
|
||||
// --- Other Useful Methods to Consider ---
|
||||
|
||||
public void ApplyForceAtWorldPosition(Vector2 force, Vector2 worldPosition)
|
||||
{
|
||||
if (IsStatic) return;
|
||||
|
||||
ForceAccumulator += force;
|
||||
Vector2 worldOffset = worldPosition - Position;
|
||||
float torque = worldOffset.X * force.Y - worldOffset.Y * force.X;
|
||||
TorqueAccumulator += torque;
|
||||
}
|
||||
|
||||
public Vector2 GetWorldPointFromLocal(Vector2 localPoint)
|
||||
{
|
||||
float cos = MathF.Cos(Rotation);
|
||||
float sin = MathF.Sin(Rotation);
|
||||
Vector2 rotated = new Vector2(
|
||||
localPoint.X * cos - localPoint.Y * sin,
|
||||
localPoint.X * sin + localPoint.Y * cos
|
||||
);
|
||||
return Position + rotated;
|
||||
}
|
||||
|
||||
public Vector2 GetLocalPointFromWorld(Vector2 worldPoint)
|
||||
{
|
||||
Vector2 delta = worldPoint - Position;
|
||||
float cos = MathF.Cos(-Rotation);
|
||||
float sin = MathF.Sin(-Rotation);
|
||||
return new Vector2(
|
||||
delta.X * cos - delta.Y * sin,
|
||||
delta.X * sin + delta.Y * cos
|
||||
);
|
||||
}
|
||||
|
||||
public void ClearForces()
|
||||
{
|
||||
ForceAccumulator = Vector2.Zero;
|
||||
TorqueAccumulator = 0.0f;
|
||||
}
|
||||
|
||||
public virtual void Draw(float pixelsPerMeter = 100.0f) {}
|
||||
public virtual void ApplyAerodynamicDrag(float dt) {}
|
||||
}
|
||||
}
|
||||
78
PhysicsEngine/Physics/Bodies/Box.cs
Normal file
78
PhysicsEngine/Physics/Bodies/Box.cs
Normal file
@@ -0,0 +1,78 @@
|
||||
using System;
|
||||
using System.Numerics;
|
||||
using Raylib_cs;
|
||||
|
||||
namespace PhysicsEngine
|
||||
{
|
||||
public class Box : Body
|
||||
{
|
||||
public Vector2 Size;
|
||||
|
||||
public Box(Vector2 position, Vector2 size, float mass)
|
||||
{
|
||||
Position = position;
|
||||
Size = size;
|
||||
Mass = mass;
|
||||
MomentOfInertia = (1.0f / 12.0f) * mass * (size.X * size.X + size.Y * size.Y);
|
||||
}
|
||||
|
||||
public override void Draw(float pixelsPerMeter = 100.0f)
|
||||
{
|
||||
DrawHelper.DrawRotatedBox(Position, Size, Rotation, BaseColor, pixelsPerMeter);
|
||||
}
|
||||
|
||||
private Vector2 RotatePoint(Vector2 point, float radians)
|
||||
{
|
||||
float cos = MathF.Cos(radians);
|
||||
float sin = MathF.Sin(radians);
|
||||
return new Vector2(
|
||||
point.X * cos - point.Y * sin,
|
||||
point.X * sin + point.Y * cos
|
||||
);
|
||||
}
|
||||
|
||||
public override void ApplyAerodynamicDrag(float dt)
|
||||
{
|
||||
if (DragCoefficient <= 0.0f) return;
|
||||
|
||||
const float airDensity = 1.225f;
|
||||
float speed = Velocity.Length();
|
||||
if (speed < 0.01f) return;
|
||||
|
||||
// 1. Oncoming relative wind direction (opposite to velocity)
|
||||
Vector2 windDir = -Velocity / speed;
|
||||
|
||||
float cos = MathF.Cos(Rotation);
|
||||
float sin = MathF.Sin(Rotation);
|
||||
|
||||
// Box local axes transformed into world space (Assuming Size.Y is length/major axis)
|
||||
Vector2 worldAxisY = new Vector2(-sin, cos); // Major axis vector
|
||||
Vector2 worldAxisX = new Vector2(cos, sin); // Minor axis vector
|
||||
|
||||
// 2. Calculate effective frontal width (projected area in 2D) based on current orientation
|
||||
Vector2 windPerp = new Vector2(-windDir.Y, windDir.X);
|
||||
float projX = MathF.Abs(Vector2.Dot(worldAxisX, windPerp));
|
||||
float projY = MathF.Abs(Vector2.Dot(worldAxisY, windPerp));
|
||||
float effectiveWidth = Size.X * projX + Size.Y * projY;
|
||||
|
||||
// 3. Translational Drag Force (scales with dynamic pressure and current effective width)
|
||||
float dragMagnitude = 0.5f * airDensity * speed * speed * DragCoefficient * effectiveWidth;
|
||||
Vector2 dragForce = windDir * dragMagnitude;
|
||||
ApplyForce(dragForce);
|
||||
|
||||
// 4. Weathercock / Fin-Effect Restoring Torque
|
||||
// Measures angular misalignment between the box's major axis and the wind direction
|
||||
float cross = worldAxisY.X * windDir.Y - worldAxisY.Y * windDir.X;
|
||||
float restoringTorque = cross * 0.5f * airDensity * speed * speed * DragCoefficient * Size.X * Size.Y;
|
||||
ApplyTorque(restoringTorque);
|
||||
|
||||
// 5. Angular Damping (Quadratic Rotational Drag)
|
||||
float angularSpeed = MathF.Abs(AngularVelocity);
|
||||
if (angularSpeed > 0.0001f)
|
||||
{
|
||||
float angularDragTorque = -MathF.Sign(AngularVelocity) * 0.5f * airDensity * angularSpeed * angularSpeed * DragCoefficient * (Size.X + Size.Y);
|
||||
ApplyTorque(angularDragTorque);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
44
PhysicsEngine/Physics/Bodies/Circle.cs
Normal file
44
PhysicsEngine/Physics/Bodies/Circle.cs
Normal file
@@ -0,0 +1,44 @@
|
||||
using System;
|
||||
using System.Numerics;
|
||||
using Raylib_cs;
|
||||
|
||||
namespace PhysicsEngine
|
||||
{
|
||||
public class Circle : Body
|
||||
{
|
||||
public float Radius;
|
||||
|
||||
public Circle(Vector2 position, float radius, float mass)
|
||||
{
|
||||
Position = position;
|
||||
Radius = radius;
|
||||
Mass = mass;
|
||||
|
||||
if (mass > 0.0f)
|
||||
{
|
||||
MomentOfInertia = 0.5f * mass * radius * radius;
|
||||
}
|
||||
}
|
||||
|
||||
public override void Draw(float pixelsPerMeter = 100.0f)
|
||||
{
|
||||
DrawHelper.DrawCircleBody(Position, Radius, Rotation, BaseColor, pixelsPerMeter);
|
||||
}
|
||||
|
||||
public override void ApplyAerodynamicDrag(float dt)
|
||||
{
|
||||
if (DragCoefficient <= 0.0f) return;
|
||||
|
||||
const float airDensity = 1.225f;
|
||||
float area = Radius * 2.0f;
|
||||
float speed = Velocity.Length();
|
||||
|
||||
if (speed > 0.0001f)
|
||||
{
|
||||
Vector2 dragDir = -Vector2.Normalize(Velocity);
|
||||
float dragMagnitude = 0.5f * airDensity * speed * speed * DragCoefficient * area;
|
||||
ApplyForce(dragDir * dragMagnitude);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user