1 files changed, 178 insertions, 57 deletions

diff --git a/src/crepe/system/CollisionSystem.cpp b/src/crepe/system/CollisionSystem.cpp
index af8adce..9604543 100644
--- a/src/crepe/system/CollisionSystem.cpp
+++ b/src/crepe/system/CollisionSystem.cpp
@@ -15,11 +15,12 @@
 #include "api/Rigidbody.h"
 #include "api/Transform.h"
 #include "api/Vector2.h"
+#include "util/AbsoluutPosition.h"
+#include "util/OptionalRef.h"
 
 #include "Collider.h"
 #include "CollisionSystem.h"
 #include "types.h"
-#include "util/OptionalRef.h"
 
 using namespace crepe;
 
@@ -63,7 +64,7 @@ void CollisionSystem::update() {
 	// For both objects call the collision handler
 	for (auto & collision_pair : collided) {
 		this->collision_handler_request(collision_pair.first, collision_pair.second);
-		this->collision_handler_request(collision_pair.second, collision_pair.first);
+		// this->collision_handler_request(collision_pair.second, collision_pair.first);
 	}
 }
 
@@ -138,10 +139,11 @@ CollisionSystem::collision_handler(CollisionInternal & data1, CollisionInternal
 				= std::get<std::reference_wrapper<BoxCollider>>(data1.collider);
 			const BoxCollider & collider2
 				= std::get<std::reference_wrapper<BoxCollider>>(data2.collider);
-			vec2 collider_pos1 = this->get_current_position(collider1.offset, data1.transform,
-															data1.rigidbody);
-			vec2 collider_pos2 = this->get_current_position(collider2.offset, data2.transform,
-															data2.rigidbody);
+
+			vec2 collider_pos1
+				= AbsoluutPosition::get_position(data1.transform, collider1.offset);
+			vec2 collider_pos2
+				= AbsoluutPosition::get_position(data2.transform, collider2.offset);
 			resolution = this->get_box_box_resolution(collider1, collider2, collider_pos1,
 													  collider_pos2);
 			break;
@@ -151,10 +153,10 @@ CollisionSystem::collision_handler(CollisionInternal & data1, CollisionInternal
 				= std::get<std::reference_wrapper<BoxCollider>>(data1.collider);
 			const CircleCollider & collider2
 				= std::get<std::reference_wrapper<CircleCollider>>(data2.collider);
-			vec2 collider_pos1 = this->get_current_position(collider1.offset, data1.transform,
-															data1.rigidbody);
-			vec2 collider_pos2 = this->get_current_position(collider2.offset, data2.transform,
-															data2.rigidbody);
+			vec2 collider_pos1
+				= AbsoluutPosition::get_position(data1.transform, collider1.offset);
+			vec2 collider_pos2
+				= AbsoluutPosition::get_position(data2.transform, collider2.offset);
 			resolution = -this->get_circle_box_resolution(collider2, collider1, collider_pos2,
 														  collider_pos1);
 			break;
@@ -164,10 +166,10 @@ CollisionSystem::collision_handler(CollisionInternal & data1, CollisionInternal
 				= std::get<std::reference_wrapper<CircleCollider>>(data1.collider);
 			const CircleCollider & collider2
 				= std::get<std::reference_wrapper<CircleCollider>>(data2.collider);
-			vec2 collider_pos1 = this->get_current_position(collider1.offset, data1.transform,
-															data1.rigidbody);
-			vec2 collider_pos2 = this->get_current_position(collider2.offset, data2.transform,
-															data2.rigidbody);
+			vec2 collider_pos1
+				= AbsoluutPosition::get_position(data1.transform, collider1.offset);
+			vec2 collider_pos2
+				= AbsoluutPosition::get_position(data2.transform, collider2.offset);
 			resolution = this->get_circle_circle_resolution(collider1, collider2,
 															collider_pos1, collider_pos2);
 			break;
@@ -177,10 +179,10 @@ CollisionSystem::collision_handler(CollisionInternal & data1, CollisionInternal
 				= std::get<std::reference_wrapper<CircleCollider>>(data1.collider);
 			const BoxCollider & collider2
 				= std::get<std::reference_wrapper<BoxCollider>>(data2.collider);
-			vec2 collider_pos1 = this->get_current_position(collider1.offset, data1.transform,
-															data1.rigidbody);
-			vec2 collider_pos2 = this->get_current_position(collider2.offset, data2.transform,
-															data2.rigidbody);
+			vec2 collider_pos1
+				= AbsoluutPosition::get_position(data1.transform, collider1.offset);
+			vec2 collider_pos2
+				= AbsoluutPosition::get_position(data2.transform, collider2.offset);
 			resolution = this->get_circle_box_resolution(collider1, collider2, collider_pos1,
 														 collider_pos2);
 			break;
@@ -302,16 +304,68 @@ vec2 CollisionSystem::get_circle_box_resolution(const CircleCollider & circle_co
 }
 
 void CollisionSystem::determine_collision_handler(CollisionInfo & info) {
-	// Check rigidbody type for static
-	if (info.this_rigidbody.data.body_type == Rigidbody::BodyType::STATIC) return;
-	// If second body is static perform the static collision handler in this system
-	if (info.other_rigidbody.data.body_type == Rigidbody::BodyType::STATIC) {
-		this->static_collision_handler(info);
+	// Inverted collision info
+	CollisionInfo inverted = {
+		.this_collider = info.other_collider,
+		.this_transform = info.other_transform,
+		.this_rigidbody = info.other_rigidbody,
+		.this_metadata = info.other_metadata,
+		.other_collider = info.this_collider,
+		.other_transform = info.this_transform,
+		.other_rigidbody = info.this_rigidbody,
+		.other_metadata = info.this_metadata,
+		.resolution = -info.resolution,
+		.resolution_direction = info.resolution_direction,
 	};
-	// Call collision event for user
+	// If both objects are static skip handle call collision script
+	if (info.this_rigidbody.data.body_type == Rigidbody::BodyType::STATIC
+		&& info.other_rigidbody.data.body_type == Rigidbody::BodyType::STATIC)
+		return;
+
+	//	First body is not dynamic
+	if (info.this_rigidbody.data.body_type != Rigidbody::BodyType::DYNAMIC) {
+		bool static_collision
+			= info.this_rigidbody.data.body_type == Rigidbody::BodyType::STATIC
+			  && info.other_rigidbody.data.body_type == Rigidbody::BodyType::DYNAMIC;
+		bool kinematic_collision
+			= info.this_rigidbody.data.body_type == Rigidbody::BodyType::KINEMATIC
+			  && info.other_rigidbody.data.body_type == Rigidbody::BodyType::DYNAMIC
+			  && info.this_rigidbody.data.kinematic_collision;
+
+		if (static_collision || kinematic_collision) {
+			// Static collision
+			this->static_collision_handler(inverted);
+		};
+		// Call scripts
+		this->call_collision_events(inverted, info);
+		return;
+	}
+
+	// Second body is not dynamic
+	if (info.other_rigidbody.data.body_type != Rigidbody::BodyType::DYNAMIC) {
+		bool static_collision
+			= info.other_rigidbody.data.body_type == Rigidbody::BodyType::STATIC;
+		bool kinematic_collision
+			= info.other_rigidbody.data.body_type == Rigidbody::BodyType::KINEMATIC
+			  && info.other_rigidbody.data.kinematic_collision;
+		if (static_collision || kinematic_collision) this->static_collision_handler(info);
+		this->call_collision_events(info, inverted);
+		return;
+	}
+
+	//dynamic
+	this->dynamic_collision_handler(info);
+	this->call_collision_events(info, inverted);
+}
+
+void CollisionSystem::call_collision_events(CollisionInfo & info,
+											CollisionInfo & info_inverted) {
 	CollisionEvent data(info);
+	CollisionEvent data_inverted(info_inverted);
 	EventManager & emgr = this->mediator.event_manager;
 	emgr.trigger_event<CollisionEvent>(data, info.this_collider.game_object_id);
+	emgr.trigger_event<CollisionEvent>(data_inverted,
+									   info_inverted.this_collider.game_object_id);
 }
 
 void CollisionSystem::static_collision_handler(CollisionInfo & info) {
@@ -363,6 +417,80 @@ void CollisionSystem::static_collision_handler(CollisionInfo & info) {
 	}
 }
 
+void CollisionSystem::dynamic_collision_handler(CollisionInfo & info) {
+	info.this_transform.position += info.resolution / 2;
+	info.other_transform.position += -(info.resolution / 2);
+
+	switch (info.resolution_direction) {
+		case Direction::BOTH:
+			if (info.this_rigidbody.data.elastisity_coefficient > 0) {
+				info.this_rigidbody.data.linear_velocity
+					= -info.this_rigidbody.data.linear_velocity
+					  * info.this_rigidbody.data.elastisity_coefficient;
+			} else {
+				info.this_rigidbody.data.linear_velocity = {0, 0};
+			}
+
+			if (info.other_rigidbody.data.elastisity_coefficient > 0) {
+				info.other_rigidbody.data.linear_velocity
+					= -info.other_rigidbody.data.linear_velocity
+					  * info.other_rigidbody.data.elastisity_coefficient;
+			} else {
+				info.other_rigidbody.data.linear_velocity = {0, 0};
+			}
+			break;
+		case Direction::Y_DIRECTION:
+			if (info.this_rigidbody.data.elastisity_coefficient > 0) {
+				info.this_rigidbody.data.linear_velocity.y
+					= -info.this_rigidbody.data.linear_velocity.y
+					  * info.this_rigidbody.data.elastisity_coefficient;
+			}
+			// Stop movement
+			else {
+				info.this_rigidbody.data.linear_velocity.y = 0;
+				info.this_transform.position.x -= info.resolution.x;
+			}
+
+			if (info.other_rigidbody.data.elastisity_coefficient > 0) {
+				info.other_rigidbody.data.linear_velocity.y
+					= -info.other_rigidbody.data.linear_velocity.y
+					  * info.other_rigidbody.data.elastisity_coefficient;
+			}
+			// Stop movement
+			else {
+				info.other_rigidbody.data.linear_velocity.y = 0;
+				info.other_transform.position.x -= info.resolution.x;
+			}
+			break;
+		case Direction::X_DIRECTION:
+			if (info.this_rigidbody.data.elastisity_coefficient > 0) {
+				info.this_rigidbody.data.linear_velocity.x
+					= -info.this_rigidbody.data.linear_velocity.x
+					  * info.this_rigidbody.data.elastisity_coefficient;
+			}
+			// Stop movement
+			else {
+				info.this_rigidbody.data.linear_velocity.x = 0;
+				info.this_transform.position.y -= info.resolution.y;
+			}
+
+			if (info.other_rigidbody.data.elastisity_coefficient > 0) {
+				info.other_rigidbody.data.linear_velocity.x
+					= -info.other_rigidbody.data.linear_velocity.x
+					  * info.other_rigidbody.data.elastisity_coefficient;
+			}
+			// Stop movement
+			else {
+				info.other_rigidbody.data.linear_velocity.x = 0;
+				info.other_transform.position.y -= info.resolution.y;
+			}
+			break;
+		case Direction::NONE:
+			// Not possible
+			break;
+	}
+}
+
 std::vector<std::pair<CollisionSystem::CollisionInternal, CollisionSystem::CollisionInternal>>
 CollisionSystem::gather_collisions(std::vector<CollisionInternal> & colliders) {
 
@@ -489,14 +617,18 @@ bool CollisionSystem::get_box_box_collision(const BoxCollider & box1, const BoxC
 											const Rigidbody & rigidbody1,
 											const Rigidbody & rigidbody2) const {
 	// Get current positions of colliders
-	vec2 final_position1 = this->get_current_position(box1.offset, transform1, rigidbody1);
-	vec2 final_position2 = this->get_current_position(box2.offset, transform2, rigidbody2);
+	vec2 final_position1 = AbsoluutPosition::get_position(transform1, box1.offset);
+	vec2 final_position2 = AbsoluutPosition::get_position(transform2, box2.offset);
+
+	// Scale dimensions
+	vec2 scaled_box1 = box1.dimensions * transform1.scale;
+	vec2 scaled_box2 = box2.dimensions * transform2.scale;
 
 	// Calculate half-extents (half width and half height)
-	float half_width1 = box1.dimensions.x / 2.0;
-	float half_height1 = box1.dimensions.y / 2.0;
-	float half_width2 = box2.dimensions.x / 2.0;
-	float half_height2 = box2.dimensions.y / 2.0;
+	float half_width1 = scaled_box1.x / 2.0;
+	float half_height1 = scaled_box1.y / 2.0;
+	float half_width2 = scaled_box2.x / 2.0;
+	float half_height2 = scaled_box2.y / 2.0;
 
 	// Check if the boxes overlap along the X and Y axes
 	return (final_position1.x + half_width1 > final_position2.x - half_width2
@@ -512,12 +644,16 @@ bool CollisionSystem::get_box_circle_collision(const BoxCollider & box1,
 											   const Rigidbody & rigidbody1,
 											   const Rigidbody & rigidbody2) const {
 	// Get current positions of colliders
-	vec2 final_position1 = this->get_current_position(box1.offset, transform1, rigidbody1);
-	vec2 final_position2 = this->get_current_position(circle2.offset, transform2, rigidbody2);
+	vec2 final_position1 = AbsoluutPosition::get_position(transform1, box1.offset);
+	vec2 final_position2 = AbsoluutPosition::get_position(transform2, circle2.offset);
+
+	// Scale dimensions
+	vec2 scaled_box = box1.dimensions * transform1.scale;
+	float scaled_circle = circle2.radius * transform2.scale;
 
 	// Calculate box half-extents
-	float half_width = box1.dimensions.x / 2.0;
-	float half_height = box1.dimensions.y / 2.0;
+	float half_width = scaled_box.x / 2.0;
+	float half_height = scaled_box.y / 2.0;
 
 	// Find the closest point on the box to the circle's center
 	float closest_x = std::max(final_position1.x - half_width,
@@ -531,7 +667,7 @@ bool CollisionSystem::get_box_circle_collision(const BoxCollider & box1,
 	float distance_squared = distance_x * distance_x + distance_y * distance_y;
 
 	// Compare distance squared with the square of the circle's radius
-	return distance_squared < circle2.radius * circle2.radius;
+	return distance_squared < scaled_circle * scaled_circle;
 }
 
 bool CollisionSystem::get_circle_circle_collision(const CircleCollider & circle1,
@@ -541,35 +677,20 @@ bool CollisionSystem::get_circle_circle_collision(const CircleCollider & circle1
 												  const Rigidbody & rigidbody1,
 												  const Rigidbody & rigidbody2) const {
 	// Get current positions of colliders
-	vec2 final_position1 = this->get_current_position(circle1.offset, transform1, rigidbody1);
-	vec2 final_position2 = this->get_current_position(circle2.offset, transform2, rigidbody2);
+	vec2 final_position1 = AbsoluutPosition::get_position(transform1, circle1.offset);
+	vec2 final_position2 = AbsoluutPosition::get_position(transform2, circle2.offset);
+
+	// Scale dimensions
+	float scaled_circle1 = circle1.radius * transform1.scale;
+	float scaled_circle2 = circle2.radius * transform2.scale;
 
 	float distance_x = final_position1.x - final_position2.x;
 	float distance_y = final_position1.y - final_position2.y;
 	float distance_squared = distance_x * distance_x + distance_y * distance_y;
 
 	// Calculate the sum of the radii
-	float radius_sum = circle1.radius + circle2.radius;
+	float radius_sum = scaled_circle1 + scaled_circle2;
 
 	// Check if the distance between the centers is less than or equal to the sum of the radii
 	return distance_squared < radius_sum * radius_sum;
 }
-
-vec2 CollisionSystem::get_current_position(const vec2 & collider_offset,
-										   const Transform & transform,
-										   const Rigidbody & rigidbody) const {
-	// Get the rotation in radians
-	float radians1 = transform.rotation * (M_PI / 180.0);
-
-	// Calculate total offset with scale
-	vec2 total_offset = (rigidbody.data.offset + collider_offset) * transform.scale;
-
-	// Rotate
-	float rotated_total_offset_x1
-		= total_offset.x * cos(radians1) - total_offset.y * sin(radians1);
-	float rotated_total_offset_y1
-		= total_offset.x * sin(radians1) + total_offset.y * cos(radians1);
-
-	// Final positions considering scaling and rotation
-	return (transform.position + vec2(rotated_total_offset_x1, rotated_total_offset_y1));
-}