BRL-CAD
world_object.cpp
Go to the documentation of this file.
1 /* W O R L D _ O B J E C T . C P P
2  * BRL-CAD
3  *
4  * Copyright (c) 2015 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 world_object.cpp
21  *
22  * Brief description
23  *
24  */
25 
26 
27 #ifdef HAVE_BULLET
28 
29 
30 #include "common.h"
31 
32 #include "world_object.hpp"
33 
34 #include <sstream>
35 #include <stdexcept>
36 
37 
38 namespace
39 {
40 
41 
42 static const std::string attribute_prefix = "simulate::";
43 
44 
45 template <typename T, void (*Destructor)(T *)>
46 struct AutoDestroyer {
47  AutoDestroyer(T *vptr) : ptr(vptr) {}
48 
49 
50  ~AutoDestroyer()
51  {
52  Destructor(ptr);
53  }
54 
55 
56  T * const ptr;
57 
58 
59 private:
60  AutoDestroyer(const AutoDestroyer &source);
61  AutoDestroyer &operator=(const AutoDestroyer &source);
62 };
63 
64 
65 template<typename Target, typename Source>
66 Target lexical_cast(Source arg,
67  const std::string &description = "bad lexical_cast")
68 {
69  std::stringstream interpreter;
70  Target result;
71 
72  if (!(interpreter << arg) ||
73  !(interpreter >> result) ||
74  !(interpreter >> std::ws).eof())
75  throw std::invalid_argument(description);
76 
77  return result;
78 }
79 
80 
81 HIDDEN std::pair<btVector3, btVector3>
82 get_bounding_box(db_i &db_instance, directory &dir)
83 {
84  btVector3 bounding_box_min, bounding_box_max;
85  {
86  point_t bb_min, bb_max;
87 
88  if (rt_bound_internal(&db_instance, &dir, bb_min, bb_max))
89  throw std::runtime_error("failed to get bounding box");
90 
91  VMOVE(bounding_box_min, bb_min);
92  VMOVE(bounding_box_max, bb_max);
93  }
94 
95  btVector3 bounding_box_dims = bounding_box_max - bounding_box_min;
96  btVector3 bounding_box_pos = bounding_box_dims / 2.0 + bounding_box_min;
97  return std::make_pair(bounding_box_pos, bounding_box_dims);
98 }
99 
100 
101 HIDDEN btScalar
102 get_volume(db_i &db_instance, directory &dir)
103 {
104  rt_db_internal internal;
105 
106  if (rt_db_get_internal(&internal, &dir, &db_instance, bn_mat_identity,
107  &rt_uniresource) < 0)
108  throw std::runtime_error("rt_db_get_internal() failed");
109 
110  AutoDestroyer<rt_db_internal, rt_db_free_internal> internal_autodestroy(
111  &internal);
112 
113  if (internal.idb_meth->ft_volume) {
114  fastf_t volume;
115  internal.idb_meth->ft_volume(&volume, &internal);
116  return volume;
117  }
118 
119  // approximate volume using the bounding box
120  btVector3 bounding_box_dims = get_bounding_box(db_instance, dir).second;
121  return bounding_box_dims.getX() * bounding_box_dims.getY() *
122  bounding_box_dims.getZ();
123 }
124 
125 
126 HIDDEN btVector3
127 deserialize_vector(const std::string &source)
128 {
129  std::istringstream stream(source);
130  btVector3 result;
131 
132  if ((stream >> std::ws).get() != '<')
133  throw std::invalid_argument("invalid vector");
134 
135  for (int i = 0; i < 3; ++i) {
136  std::string value;
137  std::getline(stream, value, i != 2 ? ',' : '>');
138  result[i] = lexical_cast<btScalar>(value, "invalid vector");
139  }
140 
141  if (stream.unget().get() != '>' || !(stream >> std::ws).eof())
142  throw std::invalid_argument("invalid vector");
143 
144  return result;
145 }
146 
147 
148 HIDDEN btRigidBody::btRigidBodyConstructionInfo
149 build_construction_info(btMotionState &motion_state,
150  btCollisionShape &collision_shape, btScalar mass)
151 {
152  btVector3 inertia;
153  collision_shape.calculateLocalInertia(mass, inertia);
154  return btRigidBody::btRigidBodyConstructionInfo(mass, &motion_state,
155  &collision_shape, inertia);
156 }
157 
158 
159 }
160 
161 
162 namespace simulate
163 {
164 
165 
166 MatrixMotionState::MatrixMotionState(mat_t matrix, const btVector3 &origin,
167  TreeUpdater &tree_updater) :
168  m_matrix(matrix),
169  m_origin(origin),
170  m_tree_updater(tree_updater)
171 {}
172 
173 
174 void
175 MatrixMotionState::getWorldTransform(btTransform &dest) const
176 {
177  /*
178  // rotation
179  // matrix decomposition
180 
181  // translation
182  btVector3 translation(0.0, 0.0, 0.0);
183  MAT_DELTAS_GET(translation, m_matrix);
184  dest.setOrigin(translation);
185  */
186 
187  btScalar bt_matrix[16];
188  MAT_TRANSPOSE(bt_matrix, m_matrix);
189  VADD2(&bt_matrix[12], &bt_matrix[12], m_origin);
190  dest.setFromOpenGLMatrix(bt_matrix);
191 }
192 
193 
194 void
195 MatrixMotionState::setWorldTransform(const btTransform &transform)
196 {
197  /*
198  // rotation
199  // matrix decomposition
200 
201  // translation
202  MAT_DELTAS_VEC(m_matrix, transform.getOrigin());
203  */
204 
205  btScalar bt_matrix[16];
206  transform.getOpenGLMatrix(bt_matrix);
207  VSUB2(&bt_matrix[12], &bt_matrix[12], m_origin);
208  MAT_TRANSPOSE(m_matrix, bt_matrix);
209 
210  m_tree_updater.mark_modified();
211 }
212 
213 
214 WorldObject *
215 WorldObject::create(db_i &db_instance, directory &vdirectory, mat_t matrix,
216  TreeUpdater &tree_updater)
217 {
218  btVector3 linear_velocity(0.0, 0.0, 0.0), angular_velocity(0.0, 0.0, 0.0);
219  btScalar mass;
220 
221  std::pair<btVector3, btVector3> bounding_box = get_bounding_box(db_instance,
222  vdirectory);
223  // apply matrix scaling
224  bounding_box.second[X] *= 1.0 / matrix[MSX];
225  bounding_box.second[Y] *= 1.0 / matrix[MSY];
226  bounding_box.second[Z] *= 1.0 / matrix[MSZ];
227  bounding_box.second *= 1.0 / matrix[MSA];
228 
229  {
230  const btScalar density = 1.0;
231  mass = density * get_volume(db_instance, vdirectory);
232 
233  bu_attribute_value_set obj_avs;
234  BU_AVS_INIT(&obj_avs);
235 
236  if (db5_get_attributes(&db_instance, &obj_avs, &vdirectory) < 0)
237  throw std::runtime_error("db5_get_attributes() failed");
238 
239  AutoDestroyer<bu_attribute_value_set, bu_avs_free> obj_avs_autodestroy(
240  &obj_avs);
241 
242  for (std::size_t i = 0; i < obj_avs.count; ++i)
243  if (attribute_prefix.compare(0, std::string::npos, obj_avs.avp[i].name,
244  attribute_prefix.size()) == 0) {
245  const std::string name = &obj_avs.avp[i].name[attribute_prefix.size()];
246  const std::string value = obj_avs.avp[i].value;
247 
248  if (name == "mass") {
249  mass = lexical_cast<btScalar>(value, "invalid attribute 'mass'");
250 
251  if (mass < 0.0) throw std::invalid_argument("invalid attribute 'mass'");
252  } else if (name == "linear_velocity") {
253  linear_velocity = deserialize_vector(value);
254  } else if (name == "angular_velocity") {
255  angular_velocity = deserialize_vector(value);
256  } else
257  throw std::invalid_argument("invalid attribute '" + name + "'");
258  }
259  }
260 
261  WorldObject *object = new WorldObject(vdirectory, matrix, tree_updater,
262  bounding_box.first, bounding_box.second, mass);
263  object->m_rigid_body.setLinearVelocity(linear_velocity);
264  object->m_rigid_body.setAngularVelocity(angular_velocity);
265  return object;
266 }
267 
268 
269 WorldObject::WorldObject(directory &vdirectory, mat_t matrix,
270  TreeUpdater &tree_updater, btVector3 bounding_box_pos,
271  btVector3 bounding_box_dims, btScalar mass) :
272  m_world(NULL),
273  m_motion_state(matrix, bounding_box_pos, tree_updater),
274  m_collision_shape(tree_updater, vdirectory.d_namep, bounding_box_dims / 2.0),
275  m_rigid_body(build_construction_info(m_motion_state, m_collision_shape, mass))
276 {}
277 
278 
279 WorldObject::~WorldObject()
280 {
281  if (m_world) m_world->removeRigidBody(&m_rigid_body);
282 }
283 
284 
285 void
286 WorldObject::add_to_world(btDiscreteDynamicsWorld &world)
287 {
288  if (m_world)
289  throw std::runtime_error("already in world");
290  else {
291  m_world = &world;
292  m_world->addRigidBody(&m_rigid_body);
293  }
294 }
295 
296 
297 }
298 
299 
300 #endif
301 
302 
303 // Local Variables:
304 // tab-width: 8
305 // mode: C++
306 // c-basic-offset: 4
307 // indent-tabs-mode: t
308 // c-file-style: "stroustrup"
309 // End:
310 // ex: shiftwidth=4 tabstop=8
Definition: raytrace.h:800
int rt_db_get_internal(struct rt_db_internal *ip, const struct directory *dp, const struct db_i *dbip, const mat_t mat, struct resource *resp)
Definition: dir.c:76
int rt_bound_internal(struct db_i *dbip, struct directory *dp, point_t rpp_min, point_t rpp_max)
Definition: bbox.c:407
const mat_t bn_mat_identity
Matrix and vector functionality.
Definition: mat.c:46
Definition: clone.c:90
Header file for the BRL-CAD common definitions.
const char * value
Definition: avs.h:62
#define HIDDEN
Definition: common.h:86
Definition: color.c:49
struct resource rt_uniresource
default. Defined in librt/globals.c
Definition: globals.c:41
struct bu_attribute_value_pair * avp
Definition: avs.h:89
#define BU_AVS_INIT(_ap)
Definition: avs.h:102
Definition: color.c:51
const char * name
Definition: avs.h:61
ustring object
int db5_get_attributes(const struct db_i *dbip, struct bu_attribute_value_set *avs, const struct directory *dp)
Definition: db5_io.c:1027
double fastf_t
Definition: defines.h:300
Definition: color.c:50