BRL-CAD
collision.cpp
Go to the documentation of this file.
1 /* C O L L I S I O N . C P P
2  * BRL-CAD
3  *
4  * Copyright (c) 2014 United States Government as represented by
5  * the U.S. Army Research Laboratory.
6  *
7  * This library is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public License
9  * version 2.1 as published by the Free Software Foundation.
10  *
11  * This library is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with this file; see the file named COPYING for more
18  * information.
19  */
20 /** @file collision.cpp
21  *
22  * Brief description
23  *
24  */
25 
26 
27 #ifdef HAVE_BULLET
28 
29 
30 #include "common.h"
31 
32 #include "collision.hpp"
33 
34 
35 namespace
36 {
37 
38 
39 HIDDEN bool
40 path_match(const char *full_path, const char *toplevel_path)
41 {
42  if (*full_path++ != '/') return false;
43 
44  full_path = strchr(full_path, '/');
45 
46  if (!full_path++) return false;
47 
48  return bu_strncmp(full_path, toplevel_path, strlen(toplevel_path)) == 0;
49 }
50 
51 
52 HIDDEN bool
53 check_path(const btCollisionObjectWrapper &body_a_wrap,
54  const btCollisionObjectWrapper &body_b_wrap,
55  const bu_ptbl &region_table)
56 {
57  const std::string body_a_name = static_cast<const simulate::RtCollisionShape *>
58  (body_a_wrap.getCollisionShape())->get_db_path();
59  const std::string body_b_name = static_cast<const simulate::RtCollisionShape *>
60  (body_b_wrap.getCollisionShape())->get_db_path();
61 
62  const char * const region1_name = reinterpret_cast<const region *>(BU_PTBL_GET(
63  &region_table, 0))->reg_name;
64  const char * const region2_name = reinterpret_cast<const region *>(BU_PTBL_GET(
65  &region_table, 1))->reg_name;
66 
67  if (!path_match(region1_name, body_a_name.c_str())
68  || !path_match(region2_name, body_b_name.c_str()))
69  if (!path_match(region2_name, body_a_name.c_str())
70  || !path_match(region1_name, body_b_name.c_str()))
71  return false;
72 
73  return true;
74 }
75 
76 
77 struct OverlapHandlerArgs {
78  btManifoldResult &result;
79  const btCollisionObjectWrapper &body_a_wrap;
80  const btCollisionObjectWrapper &body_b_wrap;
81  const btVector3 &normal_world_on_b;
82 };
83 
84 
85 HIDDEN void
86 on_multioverlap(application *app, partition *partition1, bu_ptbl *region_table,
87  partition *partition2)
88 {
89  OverlapHandlerArgs &args = *static_cast<OverlapHandlerArgs *>(app->a_uptr);
90 
91  if (!check_path(args.body_a_wrap, args.body_b_wrap, *region_table)) {
92  rt_default_multioverlap(app, partition1, region_table, partition2);
93  return;
94  }
95 
96  btVector3 point_on_a, point_on_b;
97  VJOIN1(point_on_a, app->a_ray.r_pt, partition1->pt_inhit->hit_dist,
98  app->a_ray.r_dir);
99  VJOIN1(point_on_b, app->a_ray.r_pt, partition1->pt_outhit->hit_dist,
100  app->a_ray.r_dir);
101  btScalar depth = -DIST_PT_PT(point_on_a, point_on_b);
102  args.result.addContactPoint(args.normal_world_on_b, point_on_b, depth);
103 
104  // handle the overlap
105  rt_default_multioverlap(app, partition1, region_table, partition2);
106 }
107 
108 
109 HIDDEN void
110 calculate_contact_points(btManifoldResult &result,
111  const btCollisionObjectWrapper &body_a_wrap,
112  const btCollisionObjectWrapper &body_b_wrap)
113 {
114  const btRigidBody &body_a_rb = *btRigidBody::upcast(
115  body_a_wrap.getCollisionObject());
116  const btRigidBody &body_b_rb = *btRigidBody::upcast(
117  body_b_wrap.getCollisionObject());
118 
119  // calculate the normal of the contact points as the resultant of the velocities -A and B
120  btVector3 normal_world_on_b = body_b_rb.getLinearVelocity();
121  VUNITIZE(normal_world_on_b);
122  btVector3 v = body_a_rb.getLinearVelocity();
123  VUNITIZE(v);
124  normal_world_on_b -= v;
125  VUNITIZE(normal_world_on_b);
126 
127  // shoot a circular grid of rays about `normal_world_on_b`
128  xrays *rays;
129  {
130  BU_ALLOC(rays, xrays);
131  BU_LIST_INIT(&rays->l);
132 
133  xray &center_ray = rays->ray;
134  center_ray.magic = RT_RAY_MAGIC;
135  center_ray.index = 0;
136  VMOVE(center_ray.r_dir, normal_world_on_b);
137 
138  // calculate the overlap volume between the AABBs
139  btVector3 overlap_min, overlap_max;
140  {
141  btVector3 body_a_aabb_min, body_a_aabb_max, body_b_aabb_min, body_b_aabb_max;
142  body_a_rb.getAabb(body_a_aabb_min, body_a_aabb_max);
143  body_b_rb.getAabb(body_b_aabb_min, body_b_aabb_max);
144 
145  overlap_max = body_a_aabb_max;
146  overlap_max.setMin(body_b_aabb_max);
147  overlap_min = body_a_aabb_min;
148  overlap_min.setMax(body_b_aabb_min);
149  }
150 
151  // radius of the circle of rays
152  // half of the diagonal of the overlap rpp so that rays will cover
153  // the entire volume from all orientations
154  btScalar radius = (overlap_max - overlap_min).length() / 2.0;
155 
156  // calculate the origin of the center ray
157  {
158  // center of the overlap volume
159  btVector3 overlap_center = (overlap_min + overlap_max) / 2.0;
160 
161  // step back from overlap_center, along the normal by `radius`,
162  // to ensure that rays start from outside of the overlap region
163  btVector3 center_ray_point = overlap_center - radius * normal_world_on_b;
164  VMOVE(center_ray.r_pt, center_ray_point);
165  }
166 
167  // calculate the 'up' vector
168  vect_t up_vect;
169  {
170  btVector3 up = normal_world_on_b.cross(btVector3(1.0, 0.0, 0.0));
171 
172  // use the y-axis if parallel to x-axis
173  if (up.isZero())
174  up = normal_world_on_b.cross(btVector3(0.0, 1.0, 0.0));
175 
176  VMOVE(up_vect, up);
177  }
178 
179  // equivalent to Bullet's collision tolerance (4mm after scaling is enabled)
180  const btScalar grid_size = 4.0;
181 
182  rt_gen_circular_grid(rays, &center_ray, radius, up_vect, grid_size);
183  }
184 
185  // shoot the rays
186  OverlapHandlerArgs args = {result, body_a_wrap, body_b_wrap, normal_world_on_b};
187  application app;
188  {
189  const simulate::RtCollisionShape &body_a_collision_shape =
190  *static_cast<const simulate::RtCollisionShape *>
191  (body_a_wrap.getCollisionShape());
192 
193  RT_APPLICATION_INIT(&app);
194  app.a_rt_i = &body_a_collision_shape.get_rt_instance();
196  app.a_multioverlap = on_multioverlap;
197  app.a_uptr = &args;
198  }
199 
200  xrays *entry;
201 
202  // shoot center ray
203  VMOVE(app.a_ray.r_pt, rays->ray.r_pt);
204  VMOVE(app.a_ray.r_dir, rays->ray.r_dir);
205  rt_shootray(&app);
206 
207  while (BU_LIST_WHILE(entry, xrays, &rays->l)) {
208  VMOVE(app.a_ray.r_pt, entry->ray.r_pt);
209  VMOVE(app.a_ray.r_dir, entry->ray.r_dir);
210  rt_shootray(&app);
211 
212  BU_LIST_DEQUEUE(&entry->l);
213  bu_free(entry, "xrays entry");
214  }
215 }
216 
217 
218 }
219 
220 
221 namespace simulate
222 {
223 
224 
225 RtCollisionShape::RtCollisionShape(const TreeUpdater &tree_updater,
226  const std::string &db_path, const btVector3 &half_extents) :
227  btBoxShape(half_extents),
228  m_tree_updater(tree_updater),
229  m_db_path(db_path)
230 {
231  m_shapeType = RT_SHAPE_TYPE;
232 }
233 
234 
235 const char *
236 RtCollisionShape::getName() const
237 {
238  return "RtCollisionShape";
239 }
240 
241 
242 void
243 RtCollisionShape::calculateLocalInertia(btScalar mass, btVector3 &inertia) const
244 {
245  // in most cases we can approximate the inertia tensor with that of a bounding box
246  btBoxShape::calculateLocalInertia(mass, inertia);
247  return;
248 }
249 
250 
251 std::string
252 RtCollisionShape::get_db_path() const
253 {
254  return m_db_path;
255 }
256 
257 
258 rt_i &
259 RtCollisionShape::get_rt_instance() const
260 {
261  return m_tree_updater.get_rt_instance();
262 }
263 
264 
265 btCollisionAlgorithm *
266 RtCollisionAlgorithm::CreateFunc::CreateCollisionAlgorithm(
267  btCollisionAlgorithmConstructionInfo &cinfo,
268  const btCollisionObjectWrapper *body_a_wrap,
269  const btCollisionObjectWrapper *body_b_wrap)
270 {
271  void *ptr = cinfo.m_dispatcher1->allocateCollisionAlgorithm(sizeof(
272  RtCollisionAlgorithm));
273  return new(ptr) RtCollisionAlgorithm(NULL, cinfo, body_a_wrap, body_b_wrap);
274 }
275 
276 
277 RtCollisionAlgorithm::RtCollisionAlgorithm(btPersistentManifold *manifold,
278  const btCollisionAlgorithmConstructionInfo &cinfo,
279  const btCollisionObjectWrapper *body_a_wrap,
280  const btCollisionObjectWrapper *body_b_wrap) :
281  btActivatingCollisionAlgorithm(cinfo, body_a_wrap, body_b_wrap),
282  m_owns_manifold(false),
283  m_manifold(manifold)
284 {
285  const btCollisionObject &body_a = *body_a_wrap->getCollisionObject();
286  const btCollisionObject &body_b = *body_b_wrap->getCollisionObject();
287 
288  if (!m_manifold && m_dispatcher->needsCollision(&body_a, &body_b)) {
289  m_manifold = m_dispatcher->getNewManifold(&body_a, &body_b);
290  m_owns_manifold = true;
291  }
292 }
293 
294 
295 RtCollisionAlgorithm::~RtCollisionAlgorithm()
296 {
297  if (m_owns_manifold && m_manifold)
298  m_dispatcher->releaseManifold(m_manifold);
299 }
300 
301 
302 void
303 RtCollisionAlgorithm::processCollision(const btCollisionObjectWrapper
304  *body_a_wrap,
305  const btCollisionObjectWrapper *body_b_wrap,
306  const btDispatcherInfo &, btManifoldResult *result)
307 {
308  if (!m_manifold)
309  return;
310 
311  result->setPersistentManifold(m_manifold);
312 #ifndef USE_PERSISTENT_CONTACTS
313  m_manifold->clearManifold();
314 #endif
315 
316  calculate_contact_points(*result, *body_a_wrap, *body_b_wrap);
317 
318 #ifdef USE_PERSISTENT_CONTACTS
319 
320  if (m_owns_manifold)
321  result->refreshContactPoints();
322 
323 #endif
324 }
325 
326 
327 btScalar
328 RtCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject *,
329  btCollisionObject *, const btDispatcherInfo &, btManifoldResult *)
330 {
331  return 1.0;
332 }
333 
334 
335 void
336 RtCollisionAlgorithm::getAllContactManifolds(btManifoldArray &manifold_array)
337 {
338  if (m_owns_manifold && m_manifold)
339  manifold_array.push_back(m_manifold);
340 }
341 
342 
343 }
344 
345 
346 #endif
347 
348 
349 // Local Variables:
350 // tab-width: 8
351 // mode: C++
352 // c-basic-offset: 4
353 // indent-tabs-mode: t
354 // c-file-style: "stroustrup"
355 // End:
356 // ex: shiftwidth=4 tabstop=8
struct xray a_ray
Actual ray to be shot.
Definition: raytrace.h:1583
struct hit * pt_outhit
OUT hit ptr.
Definition: raytrace.h:579
#define RT_RAY_MAGIC
Definition: magic.h:165
void rt_silent_logoverlap(struct application *ap, const struct partition *pp, const struct bu_ptbl *regiontable, const struct partition *InputHdp)
Definition: bool.c:1122
struct bu_list l
Definition: raytrace.h:233
Definition: raytrace.h:215
void(* a_multioverlap)(struct application *, struct partition *, struct bu_ptbl *, struct partition *)
called to resolve overlaps
Definition: raytrace.h:1593
Header file for the BRL-CAD common definitions.
int index
Which ray of a bundle.
Definition: raytrace.h:217
struct hit * pt_inhit
IN hit pointer.
Definition: raytrace.h:577
#define HIDDEN
Definition: common.h:86
Definition: ptbl.h:62
char * strchr(const char *sp, int c)
int bu_strncmp(const char *string1, const char *string2, size_t n)
Definition: str.c:191
struct rt_i * a_rt_i
this librt instance
Definition: raytrace.h:1588
#define BU_ALLOC(_ptr, _type)
Definition: malloc.h:223
#define BU_PTBL_GET(ptbl, i)
Definition: ptbl.h:108
void rt_default_multioverlap(struct application *ap, struct partition *pp, struct bu_ptbl *regiontable, struct partition *InputHdp)
Definition: bool.c:913
#define BU_LIST_WHILE(p, structure, hp)
Definition: list.h:410
vect_t r_dir
Direction of ray (UNIT Length)
Definition: raytrace.h:219
#define BU_LIST_INIT(_hp)
Definition: list.h:148
void(* a_logoverlap)(struct application *, const struct partition *, const struct bu_ptbl *, const struct partition *)
called to log overlaps
Definition: raytrace.h:1594
point_t r_pt
Point at which ray starts.
Definition: raytrace.h:218
int rt_gen_circular_grid(struct xrays *ray_bundle, const struct xray *center_ray, fastf_t radius, const fastf_t *up_vector, fastf_t gridsize)
Definition: mkbundle.c:93
struct xray ray
Definition: raytrace.h:234
int rt_shootray(struct application *ap)
void * a_uptr
application-specific pointer
Definition: raytrace.h:1618
void bu_free(void *ptr, const char *str)
Definition: malloc.c:328
#define BU_LIST_DEQUEUE(cur)
Definition: list.h:209
#define RT_APPLICATION_INIT(_p)
Definition: raytrace.h:1676
fastf_t hit_dist
dist from r_pt to hit_point
Definition: raytrace.h:250
uint32_t magic
Definition: raytrace.h:216