Added the Solid 3.5 sources to the blender source tree.
[blender.git] / extern / solid / src / DT_Object.cpp
1 /*
2  * SOLID - Software Library for Interference Detection
3  * 
4  * Copyright (C) 2001-2003  Dtecta.  All rights reserved.
5  *
6  * This library may be distributed under the terms of the Q Public License
7  * (QPL) as defined by Trolltech AS of Norway and appearing in the file
8  * LICENSE.QPL included in the packaging of this file.
9  *
10  * This library may be distributed and/or modified under the terms of the
11  * GNU General Public License (GPL) version 2 as published by the Free Software
12  * Foundation and appearing in the file LICENSE.GPL included in the
13  * packaging of this file.
14  *
15  * This library is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
16  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
17  *
18  * Commercial use or any other use of this library not covered by either 
19  * the QPL or the GPL requires an additional license from Dtecta. 
20  * Please contact info@dtecta.com for enquiries about the terms of commercial
21  * use of this library.
22  */
23
24 #include "DT_Object.h"
25 #include "DT_AlgoTable.h"
26 #include "DT_Convex.h" 
27 #include "DT_Complex.h" 
28 #include "DT_LineSegment.h" 
29 #include "DT_Transform.h"
30 #include "DT_Minkowski.h"
31 #include "DT_Sphere.h"
32
33 void DT_Object::setBBox() 
34 {
35         m_bbox = m_shape.bbox(m_xform, m_margin); 
36         DT_Vector3 min, max;
37         m_bbox.getMin().getValue(min);
38         m_bbox.getMax().getValue(max);
39         
40         T_ProxyList::const_iterator it;
41         for (it = m_proxies.begin(); it != m_proxies.end(); ++it) 
42         {
43                 BP_SetBBox(*it, min, max);
44         }
45 }
46
47 bool DT_Object::ray_cast(const MT_Point3& source, const MT_Point3& target, 
48                                                  MT_Scalar& lambda, MT_Vector3& normal) const 
49 {       
50         MT_Transform inv_xform = m_xform.inverse();
51         MT_Point3 local_source = inv_xform(source);
52         MT_Point3 local_target = inv_xform(target);
53         MT_Vector3 local_normal;
54
55         bool result = m_shape.ray_cast(local_source, local_target, lambda, local_normal);
56         
57         if (result) 
58         {
59                 normal = local_normal * inv_xform.getBasis();
60       MT_Scalar len = normal.length();
61                 if (len > MT_Scalar(0.0))
62       {
63          normal /= len;
64       }
65         }
66
67         return result;
68 }
69
70
71 typedef AlgoTable<Intersect> IntersectTable;
72 typedef AlgoTable<Common_point> Common_pointTable;
73 typedef AlgoTable<Penetration_depth> Penetration_depthTable;
74 typedef AlgoTable<Closest_points> Closest_pointsTable;
75
76
77 bool intersectConvexConvex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin,
78                                                    const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin,
79                            MT_Vector3& v) 
80 {
81         DT_Transform ta(a2w, (const DT_Convex&)a);
82         DT_Transform tb(b2w, (const DT_Convex&)b);
83     return intersect((a_margin > MT_Scalar(0.0) ? static_cast<const DT_Convex&>(DT_Minkowski(ta, DT_Sphere(a_margin))) : static_cast<const DT_Convex&>(ta)), 
84                                  (b_margin > MT_Scalar(0.0) ? static_cast<const DT_Convex&>(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast<const DT_Convex&>(tb)), v);
85 }
86
87 bool intersectComplexConvex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin,
88                                                     const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin,
89                             MT_Vector3& v) 
90 {
91         DT_Transform tb(b2w, (const DT_Convex&)b);
92     return intersect((const DT_Complex&)a, a2w, a_margin, 
93                              (b_margin > MT_Scalar(0.0) ? static_cast<const DT_Convex&>(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast<const DT_Convex&>(tb)), v);
94 }
95
96 bool intersectComplexComplex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin,
97                                                          const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin,
98                              MT_Vector3& v) 
99 {
100     return intersect((const DT_Complex&)a, a2w, a_margin, 
101                                          (const DT_Complex&)b, b2w, b_margin, v);
102 }
103
104 IntersectTable *intersectInitialize() 
105 {
106     IntersectTable *p = new IntersectTable;
107     p->addEntry(COMPLEX, COMPLEX, intersectComplexComplex);
108     p->addEntry(COMPLEX, CONVEX, intersectComplexConvex);
109     p->addEntry(CONVEX, CONVEX, intersectConvexConvex);
110     return p;
111 }
112
113 bool intersect(const DT_Object& a, const DT_Object& b, MT_Vector3& v) 
114 {
115     static IntersectTable *intersectTable = intersectInitialize();
116     Intersect intersect = intersectTable->lookup(a.getType(), b.getType());
117     return intersect(a.m_shape, a.m_xform, a.m_margin, 
118                              b.m_shape, b.m_xform, b.m_margin, v);
119 }
120
121 bool common_pointConvexConvex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin,
122                                                           const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin,
123                                                           MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) 
124 {
125         DT_Transform ta(a2w, (const DT_Convex&)a);
126         DT_Transform tb(b2w, (const DT_Convex&)b);
127     return common_point((a_margin > MT_Scalar(0.0) ? static_cast<const DT_Convex&>(DT_Minkowski(ta, DT_Sphere(a_margin))) : static_cast<const DT_Convex&>(ta)), 
128                                                 (b_margin > MT_Scalar(0.0) ? static_cast<const DT_Convex&>(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast<const DT_Convex&>(tb)), v, pa, pb);
129 }
130
131 bool common_pointComplexConvex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin,
132                                                            const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin,
133                                                            MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) 
134 {
135         DT_Transform tb(b2w, (const DT_Convex&)b);
136         return common_point((const DT_Complex&)a, a2w, a_margin,
137                                     (b_margin > MT_Scalar(0.0) ? static_cast<const DT_Convex&>(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast<const DT_Convex&>(tb)), v, pa, pb);
138 }
139
140 bool common_pointComplexComplex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin,
141                                                                 const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin,
142                                                                 MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) 
143 {
144     return common_point((const DT_Complex&)a, a2w, a_margin, 
145                                                 (const DT_Complex&)b, b2w, b_margin, v, pa, pb);
146 }
147
148 Common_pointTable *common_pointInitialize() 
149 {
150     Common_pointTable *p = new Common_pointTable;
151     p->addEntry(COMPLEX, COMPLEX, common_pointComplexComplex);
152     p->addEntry(COMPLEX, CONVEX, common_pointComplexConvex);
153     p->addEntry(CONVEX, CONVEX, common_pointConvexConvex);
154     return p;
155 }
156
157 bool common_point(const DT_Object& a, const DT_Object& b, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) 
158 {
159     static Common_pointTable *common_pointTable = common_pointInitialize();
160     Common_point common_point = common_pointTable->lookup(a.getType(), b.getType());
161     return common_point(a.m_shape, a.m_xform, a.m_margin, 
162                                                 b.m_shape, b.m_xform, b.m_margin, v, pa, pb);
163 }
164
165
166
167 bool penetration_depthConvexConvex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin,
168                                                                    const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin,
169                                    MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) 
170 {
171     return hybrid_penetration_depth(DT_Transform(a2w, (const DT_Convex&)a), a_margin, 
172                                                                         DT_Transform(b2w, (const DT_Convex&)b), b_margin, v, pa, pb);
173 }
174
175 bool penetration_depthComplexConvex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin,
176                                                                         const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin,
177                                     MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) 
178 {
179     return penetration_depth((const DT_Complex&)a, a2w, a_margin,
180                                                          DT_Transform(b2w, (const DT_Convex&)b), b_margin, v, pa, pb);
181 }
182
183 bool penetration_depthComplexComplex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin,
184                                                                          const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin,
185                                      MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) 
186 {
187     return penetration_depth((const DT_Complex&)a, a2w, a_margin, (const DT_Complex&)b, b2w, b_margin, v, pa, pb);
188 }
189
190 Penetration_depthTable *penetration_depthInitialize() 
191 {
192     Penetration_depthTable *p = new Penetration_depthTable;
193     p->addEntry(COMPLEX, COMPLEX, penetration_depthComplexComplex);
194     p->addEntry(COMPLEX, CONVEX, penetration_depthComplexConvex);
195     p->addEntry(CONVEX, CONVEX, penetration_depthConvexConvex);
196     return p;
197 }
198
199 bool penetration_depth(const DT_Object& a, const DT_Object& b, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) 
200 {
201     static Penetration_depthTable *penetration_depthTable = penetration_depthInitialize();
202     Penetration_depth penetration_depth = penetration_depthTable->lookup(a.getType(), b.getType());
203     return penetration_depth(a.m_shape, a.m_xform, a.m_margin, 
204                                      b.m_shape, b.m_xform, b.m_margin, v, pa, pb);
205 }
206
207
208 MT_Scalar closest_pointsConvexConvex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin,
209                                                                          const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin,
210                                                                          MT_Point3& pa, MT_Point3& pb)
211 {
212         DT_Transform ta(a2w, (const DT_Convex&)a);
213         DT_Transform tb(b2w, (const DT_Convex&)b);
214     return closest_points((a_margin > MT_Scalar(0.0) ? static_cast<const DT_Convex&>(DT_Minkowski(ta, DT_Sphere(a_margin))) : static_cast<const DT_Convex&>(ta)), 
215                                                   (b_margin > MT_Scalar(0.0) ? static_cast<const DT_Convex&>(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast<const DT_Convex&>(tb)), MT_INFINITY, pa, pb);
216 }
217
218 MT_Scalar closest_pointsComplexConvex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin,
219                                                                           const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin,
220                                                                           MT_Point3& pa, MT_Point3& pb)
221 {
222         DT_Transform tb(b2w, (const DT_Convex&)b);
223     return closest_points((const DT_Complex&)a, a2w, a_margin,
224                                                   (b_margin > MT_Scalar(0.0) ? static_cast<const DT_Convex&>(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast<const DT_Convex&>(tb)), pa, pb);
225 }
226
227 MT_Scalar closest_pointsComplexComplex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin,
228                                                                            const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin,
229                                                                            MT_Point3& pa, MT_Point3& pb) 
230 {
231     return closest_points((const DT_Complex&)a, a2w, a_margin, 
232                                                   (const DT_Complex&)b, b2w, b_margin, pa, pb);
233 }
234
235 Closest_pointsTable *closest_pointsInitialize()
236 {
237     Closest_pointsTable *p = new Closest_pointsTable;
238     p->addEntry(COMPLEX, COMPLEX, closest_pointsComplexComplex);
239     p->addEntry(COMPLEX, CONVEX, closest_pointsComplexConvex);
240     p->addEntry(CONVEX, CONVEX, closest_pointsConvexConvex);
241     return p;
242 }
243
244 MT_Scalar closest_points(const DT_Object& a, const DT_Object& b,
245                                                  MT_Point3& pa, MT_Point3& pb) 
246 {
247     static Closest_pointsTable *closest_pointsTable = closest_pointsInitialize();
248     Closest_points closest_points = closest_pointsTable->lookup(a.getType(), b.getType());
249     return closest_points(a.m_shape, a.m_xform, a.m_margin, 
250                                                   b.m_shape, b.m_xform, b.m_margin, pa, pb);
251 }
252