added support for hinge joints with anchor and axis

git-svn-id: svn://anubis/anaglym/trunk@238 99a6e188-d820-4881-8870-2d33a10e2619

Engine.cc

@@ -312,6 +312,24 @@ int Engine::addAMotor(Object * o1, Object * o2)
     return id;
 }
 
+int Engine::addHinge(Object * o1, Object * o2,
+        dReal anchor_x, dReal anchor_y, dReal anchor_z,
+        dReal axis_x, dReal axis_y, dReal axis_z)
+{
+    dBodyID b1 = 0;
+    dBodyID b2 = 0;
+    if (o1 != NULL)
+        b1 = o1->getBody();
+    if (o2 != NULL)
+        b2 = o2->getBody();
+    dJointID jid = m_world.createHinge(b1, b2,
+            anchor_x, anchor_y, anchor_z,
+            axis_x, axis_y, axis_z);
+    int id = m_next_joint_index++;
+    m_joints[id] = jid;
+    return id;
+}
+
 void Engine::setAMotorAxis(int jid, int anum, int rel,
         dReal x, dReal y, dReal z)
 {
@@ -329,6 +347,14 @@ void Engine::setAMotorNumAxes(int jid, int num_axes)
     }
 }
 
+void Engine::setAMotorAngle(int jid, int anum, dReal val)
+{
+    if (m_joints.find(jid) != m_joints.end())
+    {
+        m_world.setAMotorAngle(m_joints[jid], anum, val);
+    }
+}
+
 void Engine::setAMotorLoStop(int jid, dReal val)
 {
     if (m_joints.find(jid) != m_joints.end())

Engine.h

@@ -129,9 +129,13 @@ class Engine
         int addObject(bool is_static, OdeWorld::GeomType geom_type,
                 refptr< std::vector<float> > args);
         int addAMotor(Object * o1, Object * o2);
+        int addHinge(Object * o1, Object * o2,
+                dReal anchor_x, dReal anchor_y, dReal anchor_z,
+                dReal axis_x, dReal axis_y, dReal axis_z);
         void setAMotorAxis(int jid, int anum, int rel,
                 dReal x, dReal y, dReal z);
         void setAMotorNumAxes(int jid, int num_axes);
+        void setAMotorAngle(int jid, int anum, dReal val);
         void setAMotorLoStop(int jid, dReal val);
         void setAMotorHiStop(int jid, dReal val);
         void setAMotorVel(int jid, dReal val);

ag.cc

@@ -63,6 +63,7 @@ namespace ag
             { "clearWorld", clearWorld },
 
             { "_createAMotor", _createAMotor },
+            { "_createHinge", _createHinge },
 
             /* managed object functions */
             { "createBox", createBox },
@@ -283,6 +284,15 @@ namespace ag
         lua_setfield(L, -2, "setAxis");
         lua_pushcfunction(L, joint::setAMotorNumAxes);
         lua_setfield(L, -2, "setNumAxes");
+        lua_pushcfunction(L, joint::setAMotorAngle);
+        lua_setfield(L, -2, "setAngle");
+    }
+
+    static void createLuaHinge(lua_State * L, int id)
+    {
+        lua_newtable(L);
+        lua_pushinteger(L, id);
+        lua_setfield(L, -2, "id");
     }
 
     int _createAMotor(lua_State * L)
@@ -292,13 +302,51 @@ namespace ag
         {
             Engine::Object * o1 = object::getObject(L, 1);
             Engine::Object * o2 = object::getObject(L, 2);
-            /* it is ok if o1 or o2 are NULL, this means that the motor
+            /* it is ok if o1 or o2 are NULL, this means that the joint
              * is connected to the global environment */
-            int jid = g_engine->addAMotor(o1, o2);
+            if (o1 != NULL || o2 != NULL)
-            if (jid != 0)
             {
-                createLuaAMotor(L, jid);
+                int jid = g_engine->addAMotor(o1, o2);
-                return 1;
+                if (jid != 0)
+                {
+                    createLuaAMotor(L, jid);
+                    return 1;
+                }
+            }
+        }
+        lua_pushnil(L);
+        return 1;
+    }
+
+    int _createHinge(lua_State * L)
+    {
+        int argc = lua_gettop(L);
+        if (argc == 8
+                && lua_isnumber(L, 3)
+                && lua_isnumber(L, 4)
+                && lua_isnumber(L, 5)
+                && lua_isnumber(L, 6)
+                && lua_isnumber(L, 7)
+                && lua_isnumber(L, 8))
+        {
+            Engine::Object * o1 = object::getObject(L, 1);
+            Engine::Object * o2 = object::getObject(L, 2);
+            /* it is ok if o1 or o2 are NULL, this means that the joint
+             * is connected to the global environment */
+            if (o1 != NULL || o2 != NULL)
+            {
+                int jid = g_engine->addHinge(o1, o2,
+                        lua_tonumber(L, 3),
+                        lua_tonumber(L, 4),
+                        lua_tonumber(L, 5),
+                        lua_tonumber(L, 6),
+                        lua_tonumber(L, 7),
+                        lua_tonumber(L, 8));
+                if (jid != 0)
+                {
+                    createLuaHinge(L, jid);
+                    return 1;
+                }
             }
         }
         lua_pushnil(L);
@@ -1330,6 +1378,27 @@ namespace ag
             return 0;
         }
 
+        int setAMotorAngle(lua_State * L)
+        {
+            int argc = lua_gettop(L);
+            if (argc == 3
+                    && lua_istable(L, 1)
+                    && lua_isnumber(L, 2)
+                    && lua_isnumber(L, 3))
+            {
+                lua_getfield(L, 1, "id");
+                if (lua_isnumber(L, -1))
+                {
+                    int jid = lua_tointeger(L, -1);
+                    g_engine->setAMotorAngle(jid,
+                            lua_tointeger(L, 2),
+                            lua_tonumber(L, 3));
+                }
+                lua_pop(L, 1);
+            }
+            return 0;
+        }
+
         int setAMotorLoStop(lua_State * L)
         {
             int argc = lua_gettop(L);

ag.h

@@ -42,6 +42,7 @@ namespace ag
     int clearWorld(lua_State * L);
 
     int _createAMotor(lua_State * L);
+    int _createHinge(lua_State * L);
 
     /* 2D overlay functions */
     int drawArc(lua_State * L);
@@ -94,6 +95,7 @@ namespace ag
     {
         int setAMotorAxis(lua_State * L);
         int setAMotorNumAxes(lua_State * L);
+        int setAMotorAngle(lua_State * L);
         int setAMotorLoStop(lua_State * L);
         int setAMotorHiStop(lua_State * L);
         int setAMotorVel(lua_State * L);

ag.lua

@@ -19,7 +19,7 @@ end
 
 ag.createAMotor = function(obj1, obj2, axis0, axis1, axis2)
     local function setAMotorAxis(amotor, anum, axis)
-        local rel, x, y, z, angle = 0, 1, 0, 0, 0
+        local rel, x, y, z = 0, 1, 0, 0
         for k, v in pairs(axis) do
             if     (k == "rel") then
                 rel = v
@@ -30,7 +30,7 @@ ag.createAMotor = function(obj1, obj2, axis0, axis1, axis2)
             elseif (k == "z") then
                 z = v
             elseif (k == "angle") then
-                angle = v
+                amotor:setAngle(anum, v)
             elseif (k == "lo_stop") then
                 amotor:setLoStop(v)
             elseif (k == "hi_stop") then
@@ -85,3 +85,24 @@ ag.createAMotor = function(obj1, obj2, axis0, axis1, axis2)
     return amotor
 end
 
+ag.createHinge = function(obj1, obj2, anchor, axis)
+    local o1 = obj1
+    local o2 = obj2
+
+    if (type(obj1) == "number") then
+        if (obj1 == 0) then
+            o1 = { id = 0 }
+        end
+    end
+
+    if (type(obj2) == "number") then
+        if (obj2 == 0) then
+            o2 = { id = 0 }
+        end
+    end
+
+    return ag._createHinge(o1, o2,
+        anchor[1], anchor[2], anchor[3],
+        axis[1], axis[2], axis[3])
+end
+

tests/managed_objects.lua

@@ -39,12 +39,18 @@ function key_down_event(key)
         ag.exit()
     elseif (key == "r") then
         -- creating a rotating cube
-        local box = ag.createBox(1, 1, 1)
+        local spinner = ag.createBox(10, 1, 0.5)
-        init_obj(box)
+        spinner:setPosition(0, 0, 1)
-        ag.createAMotor(0, box,
+        spinner:setColor(1, 0.7, 0.7)
-            { x = 0, y = 0, z = 1, vel = 10, fmax = 10 })
+        spinner:setGravityMode(false)
-        local x, y, z = box:getPosition()
+        ag.createAMotor(0, spinner,
-        box:setPosition(x, y, 2)
+            { x = 0, y = 0, z = 1, vel = 5, fmax = 50 })
-        box:setGravityMode(false)
+        ag.createHinge(0, spinner, {spinner:getPosition()}, {0, 0, 1})
+        -- local box = ag.createBox(1, 1, 1)
+        -- init_obj(box)
+        -- ag.createAMotor(0, box,
+        --     { x = 0, y = 0, z = 1, vel = 0, fmax = 0 })
+        -- local x, y, z = box:getPosition()
+        -- box:setPosition(x, y, 2)
     end
 end