From 49a17e0d59d9123f5a23e4af0ae445b1db1cb1a3 Mon Sep 17 00:00:00 2001
From: John Alanbrook <john@pockle.world>
Date: Mon, 20 Feb 2023 17:10:03 +0000
Subject: [PATCH] add position calculations 2d

---
 source/engine/2dphysics.c | 21 +++++++++++++++++++++
 source/engine/2dphysics.h |  2 ++
 source/engine/ffi.c       |  9 +++++++++
 3 files changed, 32 insertions(+)

diff --git a/source/engine/2dphysics.c b/source/engine/2dphysics.c
index 630b20e..0b26df0 100644
--- a/source/engine/2dphysics.c
+++ b/source/engine/2dphysics.c
@@ -202,6 +202,27 @@ void phys2d_circledel(struct phys2d_circle *c)
     phys2d_shape_del(&c->shape);
 }
 
+cpVect world2go(struct gameobject *go, cpVect worldpos)
+{
+  worldpos = cpvsub(worldpos, cpBodyGetPosition(go->body));
+  worldpos = cpvmult(worldpos, 1/go->scale);
+  return worldpos;
+}
+
+cpVect go2world(struct gameobject *go, cpVect gopos)
+{
+  cpVect pos = cpBodyGetPosition(go->body);
+  float angle = cpBodyGetAngle(go->body);
+  cpTransform T = {0};
+  T.a = go->scale * go->flipx * cos(angle);
+  T.b = -sin(angle) * go->scale;
+  T.c = sin(angle) * go->scale;
+  T.d = go->scale * go->flipy * cos(angle);
+  T.tx = pos.x;
+  T.ty = pos.y;
+  return cpTransformPoint(T, gopos);
+}
+
 cpTransform body2transform(cpBody *body)
 {
   cpTransform T = {0};
diff --git a/source/engine/2dphysics.h b/source/engine/2dphysics.h
index d4f3fb7..6a804d4 100644
--- a/source/engine/2dphysics.h
+++ b/source/engine/2dphysics.h
@@ -131,5 +131,7 @@ struct color float2color(float *fcolor);
 void shape_gui(struct phys2d_shape *shape);
 
 void phys2d_reindex_body(cpBody *body);
+cpVect world2go(struct gameobject *go, cpVect worldpos);
+cpVect go2world(struct gameobject *go, cpVect gopos);
 
 #endif
diff --git a/source/engine/ffi.c b/source/engine/ffi.c
index fb40a32..37ddeb9 100644
--- a/source/engine/ffi.c
+++ b/source/engine/ffi.c
@@ -713,6 +713,15 @@ duk_ret_t duk_cmd(duk_context *duk) {
 	case 69:
 	  gameobject_set_sensor(duk_to_int(duk, 1), duk_to_boolean(duk,2));
 	  break;
+	
+	case 70:
+	  vect2duk(world2go(id2go(duk_to_int(duk,1)), duk2vec2(duk,2)));
+	  return 1;
+	  
+	case 71:
+	  vect2duk(go2world(id2go(duk_to_int(duk,1)),duk2vec2(duk,2)));
+	  return 1;
+	  
     }
 
     return 0;