[brlcad-commits] SF.net SVN: brlcad:[47015] brlcad/trunk/src/libged/simulate

[<abh...@us...>]

Revision: 47015
          http://brlcad.svn.sourceforge.net/brlcad/?rev=47015&view=rev
Author:   abhi2011
Date:     2011-10-02 16:07:57 +0000 (Sun, 02 Oct 2011)
Log Message:
-----------
Corrected positioning code and removed BB calculation in every iteration

Modified Paths:
--------------
    brlcad/trunk/src/libged/simulate/simcollisionalgo.cpp
    brlcad/trunk/src/libged/simulate/simulate.c
    brlcad/trunk/src/libged/simulate/simulate.h

Modified: brlcad/trunk/src/libged/simulate/simcollisionalgo.cpp
===================================================================
--- brlcad/trunk/src/libged/simulate/simcollisionalgo.cpp	2011-10-02 09:08:53 UTC (rev 47014)
+++ brlcad/trunk/src/libged/simulate/simcollisionalgo.cpp	2011-10-02 16:07:57 UTC (rev 47015)
@@ -98,7 +98,7 @@
 
 	//------------------- DEBUG ---------------------------
 
-    //Get the user pointers to struct rigid_body, for printing the body name
+/*    //Get the user pointers to struct rigid_body, for printing the body name
 	struct rigid_body *rbA = (struct rigid_body *)col0->getUserPointer();
 	struct rigid_body *rbB = (struct rigid_body *)col1->getUserPointer();
 
@@ -154,8 +154,8 @@
 
 		}
 	}
+*/
 
-
 	//------------------------------------------------------
 
 #ifdef USE_PERSISTENT_CONTACTS

Modified: brlcad/trunk/src/libged/simulate/simulate.c
===================================================================
--- brlcad/trunk/src/libged/simulate/simulate.c	2011-10-02 09:08:53 UTC (rev 47014)
+++ brlcad/trunk/src/libged/simulate/simulate.c	2011-10-02 16:07:57 UTC (rev 47015)
@@ -205,6 +205,7 @@
 
 int add_physics_attribs(struct rigid_body *current_node)
 {
+
 	VSETALL(current_node->linear_velocity, 0.0f);
 	VSETALL(current_node->angular_velocity, 0.0f);
 
@@ -343,6 +344,8 @@
 		current_node->m[12] = current_node->bb_center[0];
 		current_node->m[13] = current_node->bb_center[1];
 		current_node->m[14] = current_node->bb_center[2];
+
+		MAT_COPY(current_node->m_prev, current_node->m);
 	}
 
 	return GED_OK;
@@ -580,13 +583,17 @@
 		/* Get the internal representation of the object */
 		GED_DB_GET_INTERNAL(gedp, &intern, current_node->dp, bn_mat_identity, &rt_uniresource, GED_ERROR);
 
+		bu_log("Got this matrix for current iteration :");
 		print_matrix(current_node->dp->d_namep, current_node->m);
 
+		bu_log("Previous iteration matrix:");
+		print_matrix(current_node->dp->d_namep, current_node->m_prev);
+
 		/* Translate to origin without any rotation, before applying rotation */
 		MAT_IDN(m);
-		m[12] = - (current_node->bb_center[0]);
-		m[13] = - (current_node->bb_center[1]);
-		m[14] = - (current_node->bb_center[2]);
+		m[12] = - (current_node->m_prev[12]);
+		m[13] = - (current_node->m_prev[13]);
+		m[14] = - (current_node->m_prev[14]);
 		MAT_TRANSPOSE(t, m);
 		if (rt_matrix_transform(&intern, t, &intern, 0, gedp->ged_wdbp->dbip, &rt_uniresource) < 0){
 			bu_vls_printf(gedp->ged_result_str, "apply_transforms: ERROR rt_matrix_transform(%s) failed while \
@@ -595,6 +602,27 @@
 			return GED_ERROR;
 		}
 
+		bu_log("Translating back : %f, %f, %f", m[12], m[13], m[14]);
+		print_matrix(current_node->dp->d_namep, t);
+
+		/* Apply inverse rotation with no translation to undo previous iteration's rotation */
+		MAT_COPY(m, current_node->m_prev);
+		m[12] = 0;
+		m[13] = 0;
+		m[14] = 0;
+		MAT_COPY(t, m);
+		if (rt_matrix_transform(&intern, t, &intern, 0, gedp->ged_wdbp->dbip, &rt_uniresource) < 0){
+			bu_vls_printf(gedp->ged_result_str, "apply_transforms: ERROR rt_matrix_transform(%s) failed while \
+					applying rotation\n",
+					current_node->dp->d_namep);
+			return GED_ERROR;
+		}
+
+		bu_log("Rotating back :");
+		print_matrix(current_node->dp->d_namep, t);
+
+		/*---------------------- Now apply current transformation -------------------------*/
+
 		/* Apply rotation with no translation*/
 		MAT_COPY(m, current_node->m);
 		m[12] = 0;
@@ -608,6 +636,10 @@
 			return GED_ERROR;
 		}
 
+		bu_log("Rotating forward :");
+		print_matrix(current_node->dp->d_namep, t);
+
+
 		/* Translate again without any rotation, to apply final position */
 		MAT_IDN(m);
 		m[12] = current_node->m[12];
@@ -615,6 +647,7 @@
 		m[14] = current_node->m[14];
 		MAT_TRANSPOSE(t, m);
 
+		bu_log("Translating forward by %f, %f, %f", m[12], m[13], m[14]);
 		print_matrix(current_node->dp->d_namep, t);
 
 		if (rt_matrix_transform(&intern, t, &intern, 0, gedp->ged_wdbp->dbip, &rt_uniresource) < 0){
@@ -624,6 +657,8 @@
 			return GED_ERROR;
 		}
 
+
+
 		/* Write the modified solid to the db so it can be redrawn at the new position & orientation by Mged */
 		if (rt_db_put_internal(current_node->dp, gedp->ged_wdbp->dbip, &intern, &rt_uniresource) < 0) {
 			bu_vls_printf(gedp->ged_result_str, "apply_transforms: ERROR Database write error for '%s', aborting\n",
@@ -631,6 +666,13 @@
 			return GED_ERROR;
 		}
 
+		/*current_node->bb_center[0] = m[12];
+		current_node->bb_center[1] = m[13];
+		current_node->bb_center[2] = m[14];*/
+
+		/* Store this world transformation to undo it before next world transformation */
+		MAT_COPY(current_node->m_prev, current_node->m);
+
 		insertAABB(gedp, sim_params, current_node);
 
 		print_manifold_list(current_node);
@@ -641,7 +683,11 @@
     return GED_OK;
 }
 
-
+/**
+ * Frees the list of manifolds as generated by Bullet: this list is used
+ * to compare with rt generated manifold for accuracy
+ *
+ */
 int free_manifold_lists(struct simulation_params *sim_params)
 {
 	/* Free memory in manifold list */
@@ -713,18 +759,21 @@
 		return GED_ERROR;
 	}
 
+	rv = get_bb(gedp, &sim_params);
+	if (rv != GED_OK){
+		bu_vls_printf(gedp->ged_result_str, "%s: ERROR while getting bounding boxes\n", argv[0]);
+		return GED_ERROR;
+	}
+
     for (i=0 ; i < sim_params.duration ; i++) {
 
     	bu_log("%s: ------------------------- Iteration %d -----------------------\n", argv[0], i+1);
 
-    	rv = get_bb(gedp, &sim_params);
-    	if (rv != GED_OK){
-    		bu_vls_printf(gedp->ged_result_str, "%s: ERROR while getting bounding boxes\n", argv[0]);
-    		return GED_ERROR;
-    	}
 
-    	/* */
 
+    	/* Generate manifolds using rt */
+    	//generate_manifolds(sim_params);
+
     	/* Run the physics simulation  */
 		rv = run_simulation(&sim_params);
 		if (rv != GED_OK){
@@ -740,6 +789,7 @@
 		}
 
 		free_manifold_lists(&sim_params);
+
     }
 
 

Modified: brlcad/trunk/src/libged/simulate/simulate.h
===================================================================
--- brlcad/trunk/src/libged/simulate/simulate.h	2011-10-02 09:08:53 UTC (rev 47014)
+++ brlcad/trunk/src/libged/simulate/simulate.h	2011-10-02 16:07:57 UTC (rev 47015)
@@ -62,7 +62,7 @@
     point_t bb_center, bb_dims;     /**< @brief bb center and dimensions */
     point_t btbb_min, btbb_max;     /**< @brief Bullet body bb bounds */
     point_t btbb_center, btbb_dims; /**< @brief Bullet bb center and dimensions */
-    mat_t m;                        /**< @brief transformation matrix from Bullet */
+    mat_t m, m_prev;                        /**< @brief transformation matrix from Bullet */
     int state;						/**< @brief rigid body state from Bullet */
     struct directory *dp;           /**< @brief directory pointer to the related region */
     struct rigid_body *next;        /**< @brief link to next body */
@@ -72,8 +72,8 @@
     vect_t angular_velocity; 		/**< @brief angular velocity components */
 
     /* Manifolds in which this body participates and is body B, only body B has manifold info*/
-    int num_manifolds;
-    struct sim_manifold *first_manifold;
+    int num_manifolds;				/**< @brief angular velocity components */
+    struct sim_manifold *first_manifold; /**< @brief angular velocity components */
 };
 
 /* Contains the simulation parameters, such as number of rigid bodies,

This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.