From 798b09eb0da2758bc72d356662d03256558fba5e Mon Sep 17 00:00:00 2001
From: bjorn <tie.372@gmail.com>
Date: Mon, 15 May 2017 23:10:56 -0600
Subject: [PATCH] Body:getOrientation; Body:setOrientation;

---
 src/api/types/body.c  | 23 +++++++++++++++++++++++
 src/physics/physics.c | 14 ++++++++++++++
 src/physics/physics.h |  2 ++
 3 files changed, 39 insertions(+)

diff --git a/src/api/types/body.c b/src/api/types/body.c
index 926a1435..a338b0fe 100644
--- a/src/api/types/body.c
+++ b/src/api/types/body.c
@@ -20,8 +20,31 @@ int l_lovrBodySetPosition(lua_State* L) {
   return 0;
 }
 
+int l_lovrBodyGetOrientation(lua_State* L) {
+  Body* body = luax_checktype(L, 1, Body);
+  float angle, x, y, z;
+  lovrBodyGetOrientation(body, &angle, &x, &y, &z);
+  lua_pushnumber(L, angle);
+  lua_pushnumber(L, x);
+  lua_pushnumber(L, y);
+  lua_pushnumber(L, z);
+  return 3;
+}
+
+int l_lovrBodySetOrientation(lua_State* L) {
+  Body* body = luax_checktype(L, 1, Body);
+  float angle = luaL_checknumber(L, 2);
+  float x = luaL_checknumber(L, 3);
+  float y = luaL_checknumber(L, 4);
+  float z = luaL_checknumber(L, 5);
+  lovrBodySetOrientation(body, angle, x, y, z);
+  return 0;
+}
+
 const luaL_Reg lovrBody[] = {
   { "getPosition", l_lovrBodyGetPosition },
   { "setPosition", l_lovrBodySetPosition },
+  { "getOrientation", l_lovrBodyGetOrientation },
+  { "setOrientation", l_lovrBodySetOrientation },
   { NULL, NULL }
 };
diff --git a/src/physics/physics.c b/src/physics/physics.c
index 7890601a..1d7ff598 100644
--- a/src/physics/physics.c
+++ b/src/physics/physics.c
@@ -1,4 +1,5 @@
 #include "physics.h"
+#include "math/quat.h"
 #include <stdlib.h>
 
 void lovrPhysicsInit() {
@@ -104,3 +105,16 @@ void lovrBodyGetPosition(Body* body, float* x, float* y, float* z) {
 void lovrBodySetPosition(Body* body, float x, float y, float z) {
   dBodySetPosition(body->id, x, y, z);
 }
+
+void lovrBodyGetOrientation(Body* body, float* angle, float* x, float* y, float* z) {
+  const dReal* q = dBodyGetQuaternion(body->id);
+  float quaternion[4] = { q[0], q[1], q[2], q[3] };
+  quat_getAngleAxis(quaternion, angle, x, y, z);
+}
+
+void lovrBodySetOrientation(Body* body, float angle, float x, float y, float z) {
+  float axis[3] = { x, y, z };
+  float quaternion[4];
+  quat_fromAngleAxis(quaternion, angle, axis);
+  dBodySetQuaternion(body->id, quaternion);
+}
diff --git a/src/physics/physics.h b/src/physics/physics.h
index 76f7743a..f71650b3 100644
--- a/src/physics/physics.h
+++ b/src/physics/physics.h
@@ -31,3 +31,5 @@ Body* lovrBodyCreate();
 void lovrBodyDestroy(const Ref* ref);
 void lovrBodyGetPosition(Body* body, float* x, float* y, float* z);
 void lovrBodySetPosition(Body* body, float x, float y, float z);
+void lovrBodyGetOrientation(Body* body, float* angle, float* x, float* y, float* z);
+void lovrBodySetOrientation(Body* body, float angle, float x, float y, float z);