libmove3d
3.13.0
|
The pqp module of BioMove3D is built on top of the PQP library (http://gamma.cs.unc.edu/SSV/). It contains collision detection and distance computation functions.
typedef struct pqp_collision_grid pqp_collision_grid |
This structure is used to know which object pairs must pass the collision test. Each p3d_obj variable has a pqpID field (see p3d.h). This field is an index in the obj_obj array. For instance, obj_obj[body1->pqpID][body2->pqpID]= 1 means that the collision between body1 and body2 will be tested while obj_obj[body1->pqpID][body2->pqpID]= 0 means that it will not be tested. There is one static pqp_collision_grid declared in p3d_pqp.c. It is initialized with the pqp_create_collision_pairs() function. If objects are deleted (robot bodies or environment obstacles), it must be cleaned and reinitialized. NB: obj_obj is symetric and should be kept symetric. colliding_body1 and colliding_body2 are the two bodies that were reported as colliding in the last collision test. NB: the PQP module uses the p3d_BB of Move3D as a quick pre-test (with the p3d_BB_overlap_ functions). They must be computed before starting PQP (it is done in p3d_col_start) and updated.
void p3d_end_pqp | ( | ) |
Deallocates all the PQP_Models.
void p3d_start_pqp | ( | ) |
This function must be called to create all the structures used by detection collision when the activated collision checker is PQP. A variable of type (class) PQP_Model is associated to each p3d_obj of the environment that means obstacles as well as robot bodies. PQP only works with triangles but the function triangulates all the faces that need to be.
int pqp_activate_all_collisions | ( | ) |
Activates the collisions between all the body pairs. If two bodies are linked by a joint, the collision test between this two bodies will not be activated.
int pqp_activate_object_collision | ( | p3d_obj * | object | ) |
Activates the collision tests between a given object and any other object. If the object is linked to another object by a joint, the collision test will not be activated.
obj | object to activate collision test with |
int pqp_activate_object_environment_collision | ( | p3d_obj * | obj | ) |
Activates the collision test between the given object and the environment. The object can be a robot body or an environment obstacle.
Activates the collision test between the two given objects. If the two objects are linked by a joint, the collision test will not be activated.
int pqp_activate_robot_collision | ( | p3d_rob * | robot | ) |
Activates all the collision tests of the given robot.
int pqp_activate_robot_environment_collision | ( | p3d_rob * | robot | ) |
Activates all the collision tests between the given robot and the environment obstacles.
Activates the collision test between the given robot and object. If the object is a body of the robot and is linked by a joint to another body of the robot, the collision test between these two bodies will not be activated.
Activates the collision tests between the two given robots.
int pqp_activate_robot_selfcollision | ( | p3d_rob * | robot | ) |
Activates all the selfcollision tests of the given robot. The collision tests between two bodies that are linked by a joint or that are linked to the same with P3D_FIXED joints are not activated (see pqp_is_pair_always_inactive()).
int pqp_all_collision_test | ( | ) |
Performs all the collision tests for the current robot. Returns 1 in case of collision, 0 otherwise.
int pqp_check_collision_pair_validity | ( | ) |
Checks if the number of bodies used in the pqp_collision_grid is valid.
Copies in o1 and o2 the addresses of the two objects that were reported as colliding during the last collision test. This function must be called after a positive collision test.
Checks the collision between two p3d_obj variables. Returns 1 if there is a collision, 0 otherwise. This is the direct interface to PQP collision test function.
int pqp_create_collision_pairs | ( | ) |
Allocates the array used to know which collisions have to be tested. All the collision tests are enabled, except for the ones between two bodies linked by a joint, that are all disabled.
int pqp_create_pqpModel | ( | p3d_obj * | obj | ) |
Creates the PQP model of the given object (obstacle or robot body). The object pqpID is set to -1 by the function. NB: pqpPose will be the pose of the first p3d_poly (pol[0]) of the object. i.e. at the beginning pqpPose= pol[0]->pos0 and, at any time, the current positions of the vertices of pol[0] are pqpPose * pol[0]->poly->vertices. For the other p3d_poly, the current positions of the vertices are pqpPose * inv(pol[0]->pos0) * pol[i]->poly->vertices.
obj | pointer to the object |
int pqp_deactivate_all_collisions | ( | ) |
Deactivates the collisions between all the body pairs.
int pqp_deactivate_object_collision | ( | p3d_obj * | obj | ) |
Deactivates the collision tests between the given object and any other object.
int pqp_deactivate_object_environment_collision | ( | p3d_obj * | obj | ) |
Deactivates the collision test between the given object and the environment. The object can be a robot body or an environment obstacle.
Deactivates the collision tests between the two given objects.
int pqp_deactivate_robot_collision | ( | p3d_rob * | robot | ) |
Deactivates all the collision tests of the given robot.
int pqp_deactivate_robot_environment_collision | ( | p3d_rob * | robot | ) |
Deactivates all the collision tests between the given robot and the environment obstacles.
Deactivates the collision test between the given robot and the obstacle.
Deactivates the collision tests between the two given robots.
int pqp_deactivate_robot_selfcollision | ( | p3d_rob * | robot | ) |
Deactivates all the selfcollision tests of the given robot.
double pqp_distance | ( | p3d_obj * | o1, |
p3d_obj * | o2, | ||
p3d_vector3 | closest_point1, | ||
p3d_vector3 | closest_point2 | ||
) |
This function computes the distance between two p3d_obj variable. In case there is collision between the bodies, it returns 0. It copies in closest_point1 and closest_point2, the positions of the closest points on body 1 and body 2 respectively. This is the direct interface to PQP distance computation function.
void pqp_draw_all_models | ( | ) |
Draws the PQP models (triangles) of the all the environment objects (obstacles and robot bodies). Use this function in an OpenGL display function.
void pqp_draw_all_OBBs | ( | int | level, |
int | opengl_context | ||
) |
Draws the OBBs (Oriented Bounding Boxes), computed by PQP, of all the environment objects (obstacles and robot bodies) at the given level in the OBB tree hierarchy. Use this function in an OpenGL display function.
int pqp_draw_model | ( | p3d_obj * | object, |
double | red, | ||
double | green, | ||
double | blue | ||
) |
Draws the PQP model (triangles) of the given object. Use this function in an OpenGL display function.
int pqp_draw_OBBs | ( | p3d_obj * | object, |
int | level | ||
) |
Draws the OBBs (Oriented Bounding Boxes), computed by PQP, of the given object at the given level in the OBB tree hierarchy. Use this function in an OpenGL display function.
int pqp_draw_triangle | ( | p3d_obj * | object, |
unsigned int | index, | ||
double | red, | ||
double | green, | ||
double | blue | ||
) |
Draws a triangle of the object's PQP model. Use this function in an OpenGL display function.
object | pointer to the object |
index | index of the triangle in the triangle array of the PQP model |
red | red component of the color that will be used to display the triangle |
green | green component of the color that will be used to display the triangle |
blue | blue component of the color that will be used to display the triangle |
int pqp_fprint_collision_pairs | ( | char * | filename | ) |
Prints, in a file, for each pair of bodies, if the collision between the two bodies will be tested.
filename | name of the file to write in |
int pqp_get_obj_pos | ( | p3d_obj * | obj, |
p3d_matrix4 | pose | ||
) |
Gets the pose of an object.
obj | pointer to the object |
pose | a 4x4 matrix that will be filled with the current object pose |
Finds (if it exists) the previous body of the body given in argument, in the kinematic chain it belongs to. This is used to skip the collision tests between two bodies that are linked by a joint.
bool pqp_get_tolerance_flag | ( | void | ) |
Gets the value of the tolerance flag.
void pqp_get_tolerance_value | ( | double * | value | ) |
Gets the value of the tolerance.
Returns wether or not the collision will be tested between two objects.
int pqp_is_face_degenerate | ( | p3d_polyhedre * | polyhedron, |
unsigned int | face_index | ||
) |
A face is degenerate if it has duplicate vertices. Another case is when a face is not planar. NB: The 'return' corresponding to the case where a face is non planar has been commented because some 3D modellers (like Blender) can create models with non planar faces. This should not be an issue for the triangulation function.
Tests if the collision tests between the two objects must always be deactivated. This occurs in different situations:
int pqp_is_point_in_triangle | ( | p3d_vector2 | p, |
p3d_vector2 | a, | ||
p3d_vector2 | b, | ||
p3d_vector2 | c | ||
) |
Tests if a 2D-point is inside a triangle or not.
p | the coordinates of the point |
a | the coordinates of the triangle's first vertex |
b | the coordinates of the triangle's second vertex |
c | the coordinates of the triangle's third vertex |
int pqp_is_pure_graphic | ( | p3d_obj * | obj | ) |
Tests if the object has non graphic polyhedra.
int pqp_obj_environment_collision_test | ( | p3d_obj * | obj | ) |
Tests all the collisions between the given object and the environment obstacles. Returns 1 in case of collision, 0 otherwise.
void pqp_orthogonal_vector | ( | p3d_vector3 | v, |
p3d_vector3 | result | ||
) |
Computes a vector that is orthogonal to the vector v and normalizes it. The function returns an arbitrary choice for the orthogonal vector.
void pqp_orthonormal_basis | ( | p3d_vector3 | u, |
p3d_vector3 | v, | ||
p3d_vector3 | w | ||
) |
Computes the vectors v and w such as (u,v,w) is a direct orthonormal base. The function returns an arbitrary choice.
void pqp_p3d_to_gl_matrix | ( | p3d_matrix4 | T, |
GLfloat | mat[16] | ||
) |
Converts a p3d pose matrix to an OpenGL one.
T | the input p3d_matrix4 |
mat | a float array that will be filled |
int pqp_print_colliding_pair | ( | ) |
Prints the names the two objects that were reported as colliding during the last collision test. This function must be called after a positive collision test.
int pqp_print_collision_pairs | ( | ) |
Displays, for each pair of bodies, if the collision between the two bodies will be tested.
int pqp_robot_all_collision_test | ( | p3d_rob * | robot | ) |
Performs all the collision tests (robot-robot, robot-environment and robot self-collisions) for the given robot only. Returns 1 in case of collision, 0 otherwise.
int pqp_robot_all_no_self_collision_test | ( | p3d_rob * | robot | ) |
Performs all the collision tests (robot-robot, robot-environment) for the given robot only. Returns 1 in case of collision, 0 otherwise.
int pqp_robot_environment_collision_test | ( | p3d_rob * | robot | ) |
Tests all the collisions between the given robot and the environment obstacles. Returns 1 in case of collision, 0 otherwise.
double pqp_robot_environment_distance | ( | p3d_rob * | robot, |
p3d_vector3 | closest_point_rob, | ||
p3d_vector3 | closest_point_obst | ||
) |
Computes the minimal distance between the robot and the environment obstacles.
robot | pointer to the robot |
closest_point_rob | will be filled with the closest point on the robot (given in world coordinates) |
closest_point_obst | will be filled with the closest point on the closest obstacle (given in world coordinates) |
Tests the collisions between the given robot and the given object (that can be a body of the robot). Returns 1 in case of collision, 0 otherwise.
Tests all the collisions between the bodies of the two given robots. Returns 1 in case of collision, 0 otherwise.
Tests all the collisions between the bodies of the two given robots without testing collision between bodies whose flag contact_surface is set to 1 (TRUE). Returns 1 in case of collision, 0 otherwise.
double pqp_robot_robot_distance | ( | p3d_rob * | robot1, |
p3d_rob * | robot2, | ||
p3d_vector3 | closest_point_rob1, | ||
p3d_vector3 | closest_point_rob2 | ||
) |
Computes the minimal distance between two robots.
robot1 | pointer to the first robot |
robot2 | pointer to the second robot |
closest_point_rob1 | will be filled with the closest point on the first robot (given in world coordinates) |
closest_point_rob2 | will be filled with the closest point on the second robot (given in world coordinates) |
Computes a weighted distance between two robots. If robot r1 and r2 have bodies (b1_1, b1_2, b1_3,...) and (b2_1, b2_2, b2_3,...) with weights (w1_1, w1_2, w1_3,...) and (w2_1, w2_2, w2_3,...) (field distance_weight in p3d_obj), the computed distance will be: d= w1_1*w2_1*dist(b1_1,b2_1) + w1_1*w2_2*dist(b1_1,b2_2) + w1_1*w2_3*dist(b1_1,b2_3) + ... w1_2*w2_1*dist(b1_2,b2_1) + w1_2*w2_2*dist(b1_2,b2_2) + w1_2*w2_3*dist(b1_2,b2_3) + ... where dist(a,b) is the distance between the closest points of a and b.
robot1 | pointer to the first robot |
robot2 | pointer to the second robot |
int pqp_robot_selfcollision_test | ( | p3d_rob * | robot | ) |
Tests all the self-collisions of the given robot. Return 1 in case of collision, 0 otherwise.
void pqp_save_model | ( | p3d_obj * | object, |
const char * | filename | ||
) |
Saves the PQP model (triangles) of the given object in a file.
void pqp_set_collision_message | ( | unsigned int | set | ) |
Enables/disables the display of messages about collisions.
set | messages about collision will be displayed if set > 0, they will not if set = 0 |
int pqp_set_obj_pos | ( | p3d_obj * | obj, |
p3d_matrix4 | pose, | ||
unsigned int | update_graphics, | ||
int | opengl_context | ||
) |
Sets the pose of an object.
obj | pointer to the object |
pose | a 4x4 matrix that containts the desired object pose |
update_graphics | set to 1 to change what must be changed to ensure that Move3D will display the object with its new position (this update can take some time; do it only when it is really needed) |
Modifies the positions of the vertices of the p3d_polyhedres of the object so that Move3D will display the object with its new position: (See pqp_create_pqpModel() function to understand the following transformations)
void pqp_set_tolerance_flag | ( | bool | value | ) |
Sets the value of the tolerance flag.
void pqp_set_tolerance_value | ( | double | value | ) |
Sets the value of the tolerance.
This function returns 1 if the distance between the two bodies is <= tolerance, 0 otherwise. In most query calling this function is faster than calling pqp_distance() and then comparing the result to tolerance.
int pqp_top_OBB | ( | p3d_obj * | object, |
double & | tx, | ||
double & | ty, | ||
double & | tz, | ||
double & | ax, | ||
double & | ay, | ||
double & | az, | ||
double & | xmin, | ||
double & | xmax, | ||
double & | ymin, | ||
double & | ymax, | ||
double & | zmin, | ||
double & | zmax | ||
) |
WIP
double pqp_triangle_area | ( | p3d_vector3 | p1, |
p3d_vector3 | p2, | ||
p3d_vector3 | p3 | ||
) |
Computes and returns the area of the triangle (p1p2p3). It uses Hero of Alexandria formula.
p1 | coordinates of triangle first vertex |
p2 | coordinates of triangle second vertex |
p3 | coordinates of triangle third vertex |
pqp_triangle* pqp_triangulate_face | ( | p3d_polyhedre * | polyhedron, |
unsigned int | face_index, | ||
unsigned int * | nb_triangles | ||
) |
Triangulates the face of index k in the face array of a p3d_polyhedre. The computed triangles (indices of vertices in the vertex array of the polyhedron) are returned while the number of computed triangles (that must be the vertex number of the face minus 2) is copied in nb_triangles.
pqp_triangle* pqp_triangulate_polygon | ( | p3d_vector2 * | vertices, |
int | nb_vertices, | ||
unsigned int * | nb_triangles | ||
) |
Triangulates the polygon whose vertices are given in the corresponding array. The number of vertices is given in nb_vertices. Returns a pointer to an array of triangles (indices, in the array of vertices, of the triangle vertices). The number of triangles is written in nb_triangles and must be (nb_vertices-2). If the triangulation fails, the function returns NULL and nb_triangles is set to 0. The used algorithm is the "ear-cut algorithm".
vertices | the array of polygon vertex coordinates |
nb_vertices | the number of vertices of the polygon (size of the vertex array) |
nb_triangles | a pointer to an integer that will be filled with the number of triangles of the triangulation |