diff --git a/.gitignore b/.gitignore index e69de29..12a01e7 100644 --- a/.gitignore +++ b/.gitignore @@ -0,0 +1,2 @@ +.venv/* +__pycache__/* \ No newline at end of file diff --git a/main.py b/main.py index a98bcc3..7810307 100644 --- a/main.py +++ b/main.py @@ -49,31 +49,82 @@ def main(): physics = PhysicsSystem() + linecollider = RectCollider((495, 1)) + ball_transform = Transform(position=pg.Vector2(250,250), rotation=pi/8.0) square_transform = Transform(position=pg.Vector2(250, 100)) + #ball2_transform = Transform(position=pg.Vector2(250, 50)) ball = Square(ball_transform, 20) ball2 = Square(square_transform, 20, color=(0,255,0,255)) + #ball3 = Ball(ball2_transform, 10) + physics.add_body( RigidBody( ball_transform, RectCollider((20,20)), velocity=pg.Vector2(0,-400), - restitution=1.0, + restitution=0.2, + coef_friction=0.4 ) ) + """physics.add_body( + RigidBody( + ball2_transform, + CircleCollider(20), + velocity=pg.Vector2(0,0) + ) + )""" + physics.add_body( RigidBody( square_transform, RectCollider((20,20)), pg.Vector2(0,0), - restitution=1.0 + restitution=0.2, + coef_friction=0.2 + ) + ) + + physics.add_body( + RigidBody( + Transform(pg.Vector2(250,0)), + linecollider, + pg.Vector2(0,0), + mass=0.0 ) ) + physics.add_body( + RigidBody( + Transform(pg.Vector2(250,500)), + linecollider, + pg.Vector2(0,0), + mass=0.0 + ) + ) + + physics.add_body( + RigidBody( + Transform(pg.Vector2(0,250),rotation=pi/2.0), + linecollider, + pg.Vector2(0,0), + mass=0.0 + ) + ) + + physics.add_body( + RigidBody( + Transform(pg.Vector2(500,250),rotation=pi/2), + linecollider, + pg.Vector2(0,0), + mass=0.0 + ) + ) + while running: dt = clock.tick(144) / 1000 @@ -82,6 +133,7 @@ def main(): physics.update(dt) ball.draw(screen) ball2.draw(screen) + #ball3.draw(screen) pg.display.flip() for event in pg.event.get(): diff --git a/rigidbody.py b/rigidbody.py index 23eeb1c..01c8fce 100644 --- a/rigidbody.py +++ b/rigidbody.py @@ -14,6 +14,12 @@ class RigidBody: angular_velocity: float = 0 mass: float = 1.0 restitution: float = 0.5 + coef_friction: float = 0.6 + + def apply_impulse(self, j: pg.Vector2, point: pg.Vector2) -> None: + moment_arm = point - self.transform.global_position + self.velocity += j * self.inv_mass + self.angular_velocity += moment_arm.cross(j) * self.inv_moment_of_inertia @property def inv_mass(self) -> float: @@ -57,20 +63,30 @@ class PhysicsSystem: if b.mass != 0.0: b.transform.position -= correction + restitution = a.restitution * b.restitution + friction = a.coef_friction * b.coef_friction + tangent = collision.normal.rotate(90) + v_rel_linear = pg.Vector2(a.velocity - b.velocity) + a_w = a.angular_velocity + b_w = b.angular_velocity + for point in collision.points: r_a = point - a.transform.position r_b = point - b.transform.position - w_cross_r_a = pg.Vector2(-r_a.y * a.angular_velocity, r_a.x * a.angular_velocity) #omega is not encoded as a vector, so we pretend it is - w_cross_r_b = pg.Vector2(-r_b.y * b.angular_velocity, r_b.x * b.angular_velocity) - v_rel = a.velocity + w_cross_r_a - b.velocity - w_cross_r_b - restitution = a.restitution * b.restitution - impulse = -(1+restitution)*(v_rel.dot(collision.normal))\ + w_cross_r_a = pg.Vector2(-r_a.y * a_w, r_a.x * a_w) #omega is not encoded as a vector, so we pretend it is + w_cross_r_b = pg.Vector2(-r_b.y * b_w, r_b.x * b_w) + v_rel = v_rel_linear + w_cross_r_a - w_cross_r_b + v_rel_tangent = v_rel.dot(tangent) + + collision_impulse = -(1+restitution)*(v_rel.dot(collision.normal))\ / (a.inv_mass + b.inv_mass + (r_a.cross(collision.normal)**2 * a.inv_moment_of_inertia)\ + (r_b.cross(collision.normal)**2 * b.inv_moment_of_inertia)) / len(collision.points) - a.velocity += collision.normal*impulse*a.inv_mass - b.velocity -= collision.normal*impulse*b.inv_mass - a.angular_velocity += (r_a.cross(impulse*collision.normal)) * a.inv_moment_of_inertia - b.angular_velocity -= (r_b.cross(impulse*collision.normal)) * b.inv_moment_of_inertia + friction_impulse = (-v_rel_tangent / (a.inv_mass + b.inv_mass +\ + (r_a.cross(tangent))**2 * a.inv_moment_of_inertia +\ + (r_b.cross(tangent)**2 * b.inv_moment_of_inertia))) / len(collision.points) + friction_impulse = pg.math.clamp(friction_impulse, -abs(collision_impulse)*friction, abs(collision_impulse)*friction) + a.apply_impulse(collision.normal*collision_impulse + friction_impulse * tangent, point) + b.apply_impulse(-1 * collision.normal*collision_impulse - friction_impulse * tangent, point) diff --git a/transform.py b/transform.py index b7de6b2..cbfae92 100644 --- a/transform.py +++ b/transform.py @@ -7,7 +7,7 @@ import pygame as pg class Transform: position: pg.Vector2 - rotation: pg.Vector2 = 0.0 + rotation: float = 0.0 scale: float = 1.0 parent: 'Transform' = None