Revision: 47439
http://brlcad.svn.sourceforge.net/brlcad/?rev=47439&view=rev
Author: abhi2011
Date: 2011-11-05 07:44:10 +0000 (Sat, 05 Nov 2011)
Log Message:
-----------
Fixing some bugs in the contact generation logic.
Modified Paths:
--------------
brlcad/trunk/src/libged/simulate/simrt.c
brlcad/trunk/src/libged/simulate/simrt.h
brlcad/trunk/src/libged/simulate/simulate.c
brlcad/trunk/src/libged/simulate/simulate.h
Modified: brlcad/trunk/src/libged/simulate/simrt.c
===================================================================
--- brlcad/trunk/src/libged/simulate/simrt.c 2011-11-05 05:29:22 UTC (rev
47438)
+++ brlcad/trunk/src/libged/simulate/simrt.c 2011-11-05 07:44:10 UTC (rev
47439)
@@ -724,14 +724,14 @@
int
traverse_normalray_lists(
- struct sim_manifold *current_manifold,
+ struct sim_manifold *mf,
struct simulation_params *sim_params,
point_t pt,
point_t dir,
vect_t overlap_min,
vect_t overlap_max)
{
- int i, rv;
+ int i, j, rv;
/*struct hit_reg *hrp;*/
struct bu_vls reg_vls = BU_VLS_INIT_ZERO;
@@ -773,7 +773,7 @@
*/
/* Check if the in solid belongs to rbA */
- comb = (struct rt_comb_internal
*)current_manifold->rbA->intern.idb_ptr;
+ comb = (struct rt_comb_internal *)mf->rbA->intern.idb_ptr;
rv = check_tree_funcleaf(sim_params->gedp->ged_wdbp->dbip,
comb,
comb->tree,
@@ -784,7 +784,7 @@
continue;
/* Check if the out solid belongs to rbB */
- comb = (struct rt_comb_internal
*)current_manifold->rbB->intern.idb_ptr;
+ comb = (struct rt_comb_internal *)mf->rbB->intern.idb_ptr;
rv = check_tree_funcleaf(sim_params->gedp->ged_wdbp->dbip,
comb,
comb->tree,
@@ -796,19 +796,37 @@
depth = DIST_PT_PT(overlap_list[i].in_point,
overlap_list[i].out_point);
- bu_log("traverse_normalray_lists: Candidate contact point for
B:%s at (%f,%f,%f) , depth %f \
- n=(%f,%f,%f), at solid %s",
current_manifold->rbB->rb_namep,
-
V3ARGS(overlap_list[i].in_point),
-
depth,
+ bu_log("traverse_normalray_lists: Contact point %d for B:%s at
(%f,%f,%f) , depth %f \
+ n=(%f,%f,%f), at solid %s", mf->num_contacts +
1,
+
mf->rbB->rb_namep,
+
V3ARGS(overlap_list[i].out_point),
+
-depth,
V3ARGS(rt_result.resultant_normal_B),
overlap_list[i].insol->st_name);
- /* TODO : Maximize area of points ? , or just add all the
points and let Bullet handle it ? */
+ /* Add the points to the contacts list of the 1st manifold,
without maximizing area here
+ * Bullet should handle contact point minimization and area
maximization
+ */
+ if(mf->num_contacts == MAX_CONTACTS_PER_MANIFOLD){
+ bu_log("Contact point skipped as %d points are the
maximum allowed per manifold", mf->num_contacts);
+ continue;
+ }
- }
+ j = mf->num_contacts;
+ VMOVE(mf->contacts[j].ptB, overlap_list[i].out_point);
+ VMOVE(mf->contacts[j].normalWorldOnB,
rt_result.resultant_normal_B);
+ mf->contacts[j].depth = -depth;
+ mf->num_contacts++;
+ bu_log("traverse_normalray_lists : ptB set to (%f,%f,%f)",
V3ARGS((mf->contacts[j].ptB)));
+ bu_log("traverse_normalray_lists : normalWorldOnB set to
(%f,%f,%f)", V3ARGS(mf->contacts[j].normalWorldOnB));
+ bu_log("traverse_normalray_lists : Penetration depth set to
%f", mf->contacts[j].depth);
+
+ } /* end-for overlap */
+
+
bu_vls_free(®_vls);
return GED_OK;
@@ -1056,10 +1074,6 @@
}
-/*
- * Shoots a circular bunch of rays from B towards A along resultant_normal_B
- *
- */
int
shoot_normal_rays(struct sim_manifold *current_manifold,
struct simulation_params *sim_params,
@@ -1171,31 +1185,6 @@
/* Shoot rays along the normal to get points on B and the depth along
this direction */
shoot_normal_rays(mf, sim_params, overlap_min, overlap_max);
- /* VSET(mf->contacts[0].normalWorldOnB, 0, 0, 1.0000);
- VSET(mf->contacts[1].normalWorldOnB, 0, 0, 1.0000);
- VSET(mf->contacts[2].normalWorldOnB, 0, 0, 1.0000);
- VSET(mf->contacts[3].normalWorldOnB, 0, 0, 1.0000);
-
-
-
- VSUB2(diff, overlap_min, overlap_max);
- mf->contacts[0].depth = diff[Z];
- mf->contacts[1].depth = diff[Z];
- mf->contacts[2].depth = diff[Z];
- mf->contacts[3].depth = diff[Z];
-
- VSET(mf->contacts[0].ptB, 0.000000, 1.000000, overlap_max[Z]);
- VSET(mf->contacts[1].ptB, 0.000000, 0.300000, overlap_max[Z]);
- VSET(mf->contacts[2].ptB, 1.000000, 0.300000, overlap_max[Z]);
- VSET(mf->contacts[3].ptB, 1.000000, 1.000000, overlap_max[Z]);*/
-
-
-
-
- /*bu_log("create_contact_pairs : ptB set to (%f,%f,%f)",
V3ARGS((mf->contacts[0].ptB)));
- bu_log("create_contact_pairs : normalWorldOnB set to (%f,%f,%f)",
V3ARGS(mf->contacts[0].normalWorldOnB));
- bu_log("create_contact_pairs : Penetration depth set to %f",
mf->contacts[0].depth);*/
-
return GED_OK;
}
@@ -1206,11 +1195,12 @@
struct rigid_body *rbB)
{
/* The manifold between rbA & rbB will be stored in B */
- struct sim_manifold *rt_mf = &(rbB->rt_manifold);
+ struct sim_manifold *rt_mf;
vect_t overlap_min, overlap_max;
char *prefix_overlap = "overlap_";
struct bu_vls overlap_name = BU_VLS_INIT_ZERO;
+ rt_mf = &(rbB->rt_manifold);
/* Setup the manifold participant pointers */
rt_mf->rbA = rbA;
@@ -1237,6 +1227,8 @@
/* Shoot rays right here as the pair of rigid_body ptrs are known,
* TODO: ignore volumes already shot
+ * UNUSED currently as velocity used to decide normal. Shooting rays in
3D is too time
+ * consuming, but may be useful in some cases in the future
*/
/*shoot_x_rays(rt_mf, sim_params, overlap_min, overlap_max);
shoot_y_rays(rt_mf, sim_params, overlap_min, overlap_max);
@@ -1244,10 +1236,9 @@
- /* Create the contact pairs and normals */
+ /* Create the contact pairs and normals : Currently just 1 manifold is
allowed per pair of objects*/
create_contact_pairs(rt_mf, sim_params, overlap_min, overlap_max);
-
return GED_OK;
}
Modified: brlcad/trunk/src/libged/simulate/simrt.h
===================================================================
--- brlcad/trunk/src/libged/simulate/simrt.h 2011-11-05 05:29:22 UTC (rev
47438)
+++ brlcad/trunk/src/libged/simulate/simrt.h 2011-11-05 07:44:10 UTC (rev
47439)
@@ -244,6 +244,17 @@
vect_t overlap_max);
+/*
+ * Shoots a circular bunch of rays from B towards A along resultant_normal_B
+ *
+ */
+int
+shoot_normal_rays(struct sim_manifold *current_manifold,
+ struct simulation_params *sim_params,
+ vect_t overlap_min,
+ vect_t overlap_max);
+
+
/**
* Traverse the hit list and overlap list, drawing the ray segments
* for x-rays
Modified: brlcad/trunk/src/libged/simulate/simulate.c
===================================================================
--- brlcad/trunk/src/libged/simulate/simulate.c 2011-11-05 05:29:22 UTC (rev
47438)
+++ brlcad/trunk/src/libged/simulate/simulate.c 2011-11-05 07:44:10 UTC (rev
47439)
@@ -63,7 +63,6 @@
VSETALL(current_node->angular_velocity, 0.0f);
current_node->num_bt_manifolds = 0;
- current_node->num_rt_manifolds = 0;
return GED_OK;
}
@@ -240,106 +239,104 @@
for (current_node = sim_params->head_node; current_node != NULL;
current_node = current_node->next) {
- /*if (strcmp(current_node->rb_namep, sim_params->ground_plane_name) ==
0)
- continue;*/
+ /*if (strcmp(current_node->rb_namep,
sim_params->ground_plane_name) == 0)
+ continue;*/
- /* Get the internal representation of the object */
- GED_DB_GET_INTERNAL(gedp, &intern, current_node->dp, bn_mat_identity,
&rt_uniresource, GED_ERROR);
+ /* 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("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); */
+ /*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->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 \
- translating to origin!\n",
- current_node->dp->d_namep);
- return GED_ERROR;
- }
+ /* Translate to origin without any rotation, before applying
rotation */
+ MAT_IDN(m);
+ 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 \
+ translating to origin!\n",
+ current_node->dp->d_namep);
+ return GED_ERROR;
+ }
- /*bu_log("Translating back : %f, %f, %f", m[12], m[13], m[14]);
- print_matrix(current_node->dp->d_namep, t); */
+ /*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;
- }
+ /* 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);*/
+ /*bu_log("Rotating back :");
+ print_matrix(current_node->dp->d_namep, t);*/
- /*---------------------- Now apply current transformation
-------------------------*/
+ /*---------------------- Now apply current transformation
-------------------------*/
- /* Apply rotation with no translation*/
- MAT_COPY(m, current_node->m);
- m[12] = 0;
- m[13] = 0;
- m[14] = 0;
- 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 \
- applying rotation\n",
- current_node->dp->d_namep);
- return GED_ERROR;
- }
+ /* Apply rotation with no translation*/
+ MAT_COPY(m, current_node->m);
+ m[12] = 0;
+ m[13] = 0;
+ m[14] = 0;
+ 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 \
+ applying rotation\n",
+ current_node->dp->d_namep);
+ return GED_ERROR;
+ }
- /*bu_log("Rotating forward :");
- print_matrix(current_node->dp->d_namep, t);*/
+ /*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];
- m[13] = current_node->m[13];
- m[14] = current_node->m[14];
- MAT_TRANSPOSE(t, m);
+ /* Translate again without any rotation, to apply final
position */
+ MAT_IDN(m);
+ m[12] = current_node->m[12];
+ m[13] = current_node->m[13];
+ 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);*/
+ /*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) {
- bu_vls_printf(gedp->ged_result_str, "apply_transforms: ERROR
rt_matrix_transform(%s) failed while \
- translating to final position\n",
- current_node->dp->d_namep);
- return GED_ERROR;
- }
+ 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 \
+ translating to final position\n",
+ current_node->dp->d_namep);
+ 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",
- current_node->dp->d_namep);
- 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",
+ current_node->dp->d_namep);
+ return GED_ERROR;
+ }
- /* Store this world transformation to undo it before next world
transformation */
- MAT_COPY(current_node->m_prev, current_node->m);
+ /* Store this world transformation to undo it before next world
transformation */
+ MAT_COPY(current_node->m_prev, current_node->m);
- insert_AABB(gedp, sim_params, current_node);
+ insert_AABB(gedp, sim_params, current_node);
- print_manifold_list(current_node);
+ print_manifold_list(current_node);
- insert_manifolds(gedp, sim_params, current_node);
+ insert_manifolds(gedp, sim_params, current_node);
- current_node->num_bt_manifolds = 0;
- current_node->num_rt_manifolds = 0;
+ current_node->num_bt_manifolds = 0;
-
}
return GED_OK;
Modified: brlcad/trunk/src/libged/simulate/simulate.h
===================================================================
--- brlcad/trunk/src/libged/simulate/simulate.h 2011-11-05 05:29:22 UTC (rev
47438)
+++ brlcad/trunk/src/libged/simulate/simulate.h 2011-11-05 07:44:10 UTC (rev
47439)
@@ -100,8 +100,9 @@
int num_bt_manifolds;
/**< @brief number of manifolds for this body */
struct sim_manifold bt_manifold[MAX_MANIFOLDS]; /**< @brief the manifolds
for this body */
- /* Manifold generated by RT, where this body is B, only body B has
manifold info */
- int num_rt_manifolds; /**< @brief number of
manifolds for this body */
+ /* Manifold generated by RT, where this body is B, ONLY 1 needed as its
injected into the physics
+ * pipeline immediately after creation thus allowing the next one to
replace it
+ */
struct sim_manifold rt_manifold; /**< @brief only 1 manifold struct needed
as the manifold is used
immediately after being generated in the nearphase callback*/
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
RSA(R) Conference 2012
Save $700 by Nov 18
Register now
http://p.sf.net/sfu/rsa-sfdev2dev1
_______________________________________________
BRL-CAD Source Commits mailing list
brlcad-commits@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/brlcad-commits
