fixed rotation and translation issues for mesh and hull collision shapes

Extras/MayaPlugin/bt_convex_hull_shape.h

@@ -37,6 +37,12 @@ public:
 
         if(m_vertices.empty() || m_indices.empty()) return;
 
+        glPushMatrix();
+        glTranslatef(m_center[0], m_center[1], m_center[2]);
+        float angle;
+        vec3f axis;
+        q_to_axis_angle(m_rotation, axis, angle);
+        glRotatef(rad2deg(angle), axis[0], axis[1], axis[2]);
         glEnableClientState(GL_VERTEX_ARRAY);
         glEnableClientState(GL_NORMAL_ARRAY);
         if(draw_style & collision_shape_t::kDSSolid) {
@@ -50,6 +56,7 @@ public:
         glDisableClientState(GL_VERTEX_ARRAY);
         glDisableClientState(GL_NORMAL_ARRAY);
         glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+        glPopMatrix();
     }
 
     virtual void set_scale(vec3f const& s) {
@@ -94,17 +101,27 @@ protected:
             m_vertices[i] = prod(Qinv, vertices[i] - m_center);
         }
 
-        set_shape(new btConvexHullShape((float const*)&(m_vertices[0]), num_vertices, sizeof(vec3f)));
+        m_ch_shape.reset(new btConvexHullShape((float const*)&(m_vertices[0]), num_vertices, sizeof(vec3f)));
+        btCompoundShape *compound_shape = new btCompoundShape;
+        compound_shape->addChildShape(btTransform(btQuaternion(m_rotation[1],
+                                                               m_rotation[2],
+                                                               m_rotation[3],
+                                                               m_rotation[0]),
+                                                  btVector3(m_center[0],
+                                                            m_center[1],
+                                                            m_center[2])),
+                                      m_ch_shape.get());
+        set_shape(compound_shape);
     }
 
     void update()
     {
-        btConvexHullShape *cu_shape = static_cast<btConvexHullShape*>(shape());
+        btConvexHullShape *ch_shape = static_cast<btConvexHullShape*>(shape());
 
         //apply the scaling
-        btVector3 const& scale = cu_shape->getLocalScaling();
+        btVector3 const& scale = m_ch_shape->getLocalScaling();
-        btPoint3 const* points = cu_shape->getPoints();
+        btPoint3 const* points = m_ch_shape->getPoints();
-        for(int i = 0; i < cu_shape->getNumPoints(); ++i) {
+        for(int i = 0; i < m_ch_shape->getNumPoints(); ++i) {
             m_vertices[i] = vec3f(scale.x() * points[i].x(), scale.y() * points[i].y(), scale.z() * points[i].z()); 
         }
         m_volume = ::volume(&(m_vertices[0]), (int*)&(m_indices[0]), m_indices.size());
@@ -123,16 +140,15 @@ protected:
     }
 
 private:
-    std::vector<vec3f> m_vertices;
+    shared_ptr<btConvexHullShape>   m_ch_shape;
-    std::vector<vec3f> m_normals;
+    std::vector<vec3f>              m_vertices;
-    std::vector<unsigned int> m_indices; 
+    std::vector<vec3f>              m_normals;
-
+    std::vector<unsigned int>       m_indices; 
-    float m_volume;
-    vec3f m_center;
-    quatf m_rotation;
-    vec3f m_local_inertia;
-
 
+    float                           m_volume;
+    vec3f                           m_center;
+    quatf                           m_rotation;
+    vec3f                           m_local_inertia;
 };
 
 #endif

Extras/MayaPlugin/bt_mesh_shape.h

@@ -40,7 +40,11 @@ public:
 
         btVector3 const& scale = shape()->getLocalScaling();
         glPushMatrix();
-        //glTranslatef(m_center[0], m_center[1], m_center[2]);
+        glTranslatef(m_center[0], m_center[1], m_center[2]);
+        float angle;
+        vec3f axis;
+        q_to_axis_angle(m_rotation, axis, angle);
+        glRotatef(rad2deg(angle), axis[0], axis[1], axis[2]);
         glScalef(scale.x(), scale.y(), scale.z());
         glEnableClientState(GL_VERTEX_ARRAY);
         glEnableClientState(GL_NORMAL_ARRAY);
@@ -105,10 +109,19 @@ protected:
         m_tiva.reset(new btTriangleIndexVertexArray(num_indices / 3, (int*)&(m_indices[0]), 3 * sizeof(unsigned int),
                                                     num_vertices, (float*)&(m_vertices[0]), sizeof(vec3f)));
 
-        btGImpactMeshShape* gimpactShape = new btGImpactMeshShape(m_tiva.get());
+        m_gi_shape.reset(new btGImpactMeshShape(m_tiva.get()));
-	gimpactShape->setLocalScaling(btVector3(1.0f,1.0f,1.0f));
+	m_gi_shape->setLocalScaling(btVector3(1.0f,1.0f,1.0f));
-        gimpactShape->updateBound();
+        m_gi_shape->updateBound();
-        set_shape(gimpactShape);
+        btCompoundShape *compound_shape = new btCompoundShape;
+        compound_shape->addChildShape(btTransform(btQuaternion(m_rotation[1],
+                                                               m_rotation[2],
+                                                               m_rotation[3],
+                                                               m_rotation[0]),
+                                                  btVector3(m_center[0],
+                                                            m_center[1],
+                                                            m_center[2])),
+                                      m_gi_shape.get());
+        set_shape(compound_shape);
 
         //std::cout << "construtor: " << m_center << std::endl;
 
@@ -117,10 +130,8 @@ protected:
 
     void update()
     {
-        btGImpactMeshShape *gi_shape = static_cast<btGImpactMeshShape*>(shape());
-
         //apply the scaling
-        btVector3 const& scale = gi_shape->getLocalScaling();
+        btVector3 const& scale = m_gi_shape->getLocalScaling();
 
         std::vector<vec3f> vertices(m_vertices.size());
         for(int i = 0; i < vertices.size(); ++i) {
@@ -142,10 +153,11 @@ protected:
     }
 
 private:
-    std::vector<vec3f> m_vertices;
+    shared_ptr<btGImpactMeshShape>          m_gi_shape;
-    std::vector<vec3f> m_normals;
+    std::vector<vec3f>                      m_vertices;
-    std::vector<unsigned int> m_indices; 
+    std::vector<vec3f>                      m_normals;
-    shared_ptr<btTriangleIndexVertexArray> m_tiva;
+    std::vector<unsigned int>               m_indices; 
+    shared_ptr<btTriangleIndexVertexArray>  m_tiva;
 
     float m_volume;
     vec3f m_center;

Extras/MayaPlugin/mvl/quat.h

@@ -121,22 +121,21 @@ q_from_axis_angle(vec<T1, 3> const& axis, T2 theta)  {
 template<typename T1, typename T2, typename T3> 
 inline 
 void
-q_to_axis_angle(vec<T1, 4> const& q, vec<T2, 3>& axis, T3& theta) {
+q_to_axis_angle(vec<T1, 4> const& q, vec<T2, 3>& axis, T3& theta)
+{
+    T3 half_theta= acos(q[0]);
 	
-	T3 half_theta= acos(q[0]);
+    if(half_theta > 10 * std::numeric_limits<T3>::epsilon()) {
+        T3 oost = 1 / sin(half_theta);
 		
-	if(half_theta > 10 * std::numeric_limits<T3>::epsilon()) {
+	axis[0] = oost * q[1];
-		
+	axis[1] = oost * q[2];
-		T3 oost = 1 / sin(half_theta);
+	axis[2] = oost * q[3];
-		
+	theta = 2 * half_theta;
-		axis[0] = oost * q[1];
+    } else {
-		axis[1] = oost * q[2];
-		axis[2] = oost * q[3];
-		theta = 2 * half_theta;
-	} else {
         axis[0] = axis[1] = axis[2] = 0;
-		theta = 0;
+        theta = 0;
-	}
+    }
 }
 
 //init quaternion from rotation matrix

Extras/MayaPlugin/shared_ptr.h

@@ -190,8 +190,8 @@ protected:
     }
 
 protected:
-    T           *m_ptr;
+    T               *m_ptr;
-   shared_count *m_count;
+    shared_count    *m_count;
 
 };