1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2014 projectchrono.org
5 // All rights reserved.
6 //
7 // Use of this source code is governed by a BSD-style license that can be found
8 // in the LICENSE file at the top level of the distribution and at
9 // http://projectchrono.org/license-chrono.txt.
10 //
11 // =============================================================================
12
13 #include <cstdio>
14 #include <cstring>
15
16 #include "chrono/collision/edgetempest/ChCMatVec.h"
17 #include "chrono/collision/edgetempest/ChCGetTime.h"
18 #include "chrono/collision/edgetempest/ChCCollisionTree.h"
19 #include "chrono/physics/ChBody.h"
20
21 namespace chrono {
22 namespace collision {
23
ChCollisionTree()24 ChCollisionTree::ChCollisionTree() {
25 // no bounding volume tree yet
26 num_geometries = 0;
27
28 last_geometry = NULL;
29
30 build_state = ChC_BUILD_STATE_MODIFIED;
31
32 m_body = NULL;
33 }
34
~ChCollisionTree()35 ChCollisionTree::~ChCollisionTree() {
36 ResetModel();
37 }
38
ResetModel()39 int ChCollisionTree::ResetModel() {
40 // delete previously added geometries
41 std::vector<geometry::ChGeometry*>::iterator nit = geometries.begin();
42 for (; nit != geometries.end(); ++nit) {
43 if (*nit)
44 delete (*nit);
45 *nit = NULL;
46 }
47 geometries.clear();
48 num_geometries = 0;
49
50 build_state = ChC_BUILD_STATE_MODIFIED;
51
52 return ChC_OK;
53 }
54
BuildModel(double envelope)55 int ChCollisionTree::BuildModel(double envelope) {
56 if (build_state == ChC_BUILD_STATE_PROCESSED) {
57 // fprintf(stderr,"Warning! Called EndModel() on ChCollisionTree \n"
58 // "object that was already ended. EndModel() was\n"
59 // "ignored. Must do a BeginModel() to clear the\n"
60 // "model for addition of new triangles\n");
61 return ChC_OK;
62 }
63
64 if (num_geometries)
65 last_geometry = *geometries.begin();
66 else
67 last_geometry = NULL;
68
69 return ChC_OK;
70 }
71
AddGeometry(geometry::ChGeometry * mgeo)72 int ChCollisionTree::AddGeometry(geometry::ChGeometry* mgeo) {
73 this->geometries.push_back(mgeo);
74
75 num_geometries += 1;
76
77 build_state = ChC_BUILD_STATE_MODIFIED;
78
79 return ChC_OK;
80 }
81
GetBoundingBox(double & xmin,double & xmax,double & ymin,double & ymax,double & zmin,double & zmax,ChMatrix33<> * Rot)82 void ChCollisionTree::GetBoundingBox(double& xmin,
83 double& xmax,
84 double& ymin,
85 double& ymax,
86 double& zmin,
87 double& zmax,
88 ChMatrix33<>* Rot) {
89 xmin = ymin = zmin = +10e20;
90 xmax = ymax = zmax = -10e20;
91
92 std::vector<geometry::ChGeometry*>::iterator nit = geometries.begin();
93 for (; nit != geometries.end(); ++nit) {
94 if ((*nit)) {
95 (*nit)->InflateBoundingBox(xmin, xmax, ymin, ymax, zmin, zmax, Rot);
96 }
97 }
98 }
99
UpdateAbsoluteAABB(double envelope)100 void ChCollisionTree::UpdateAbsoluteAABB(double envelope) {
101 assert(m_body);
102
103 double xmin, xmax, ymin, ymax, zmin, zmax;
104
105 m_absoluteAABB.init(this);
106
107 static ChMatrix33<> at = m_body->GetA().transpose();
108
109 GetBoundingBox(xmin, xmax, ymin, ymax, zmin, zmax, &at);
110
111 m_absoluteAABB.m_beginX.m_value = xmin - envelope + m_body->GetPos().x();
112 m_absoluteAABB.m_endX.m_value = xmax + envelope + m_body->GetPos().x();
113 m_absoluteAABB.m_beginY.m_value = ymin - envelope + m_body->GetPos().y();
114 m_absoluteAABB.m_endY.m_value = ymax + envelope + m_body->GetPos().y();
115 m_absoluteAABB.m_beginZ.m_value = zmin - envelope + m_body->GetPos().z();
116 m_absoluteAABB.m_endZ.m_value = zmax + envelope + m_body->GetPos().z();
117 }
118
119 } // end namespace collision
120 } // end namespace chrono
121