|
1 /************************************************************************* |
|
2 * * |
|
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * |
|
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org * |
|
5 * * |
|
6 * This library is free software; you can redistribute it and/or * |
|
7 * modify it under the terms of EITHER: * |
|
8 * (1) The GNU Lesser General Public License as published by the Free * |
|
9 * Software Foundation; either version 2.1 of the License, or (at * |
|
10 * your option) any later version. The text of the GNU Lesser * |
|
11 * General Public License is included with this library in the * |
|
12 * file LICENSE.TXT. * |
|
13 * (2) The BSD-style license that is included with this library in * |
|
14 * the file LICENSE-BSD.TXT. * |
|
15 * * |
|
16 * This library is distributed in the hope that it will be useful, * |
|
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
|
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * |
|
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. * |
|
20 * * |
|
21 *************************************************************************/ |
|
22 |
|
23 /* |
|
24 |
|
25 standard ODE geometry primitives: public API and pairwise collision functions. |
|
26 |
|
27 the rule is that only the low level primitive collision functions should set |
|
28 dContactGeom::g1 and dContactGeom::g2. |
|
29 |
|
30 */ |
|
31 |
|
32 #include <ode/common.h> |
|
33 #include <ode/collision.h> |
|
34 #include <ode/matrix.h> |
|
35 #include <ode/rotation.h> |
|
36 #include <ode/odemath.h> |
|
37 #include "collision_kernel.h" |
|
38 #include "collision_std.h" |
|
39 #include "collision_util.h" |
|
40 |
|
41 //**************************************************************************** |
|
42 // plane public API |
|
43 |
|
44 static void make_sure_plane_normal_has_unit_length (dxPlane *g) |
|
45 { |
|
46 dReal l = dMUL(g->p[0],g->p[0]) + dMUL(g->p[1],g->p[1]) + dMUL(g->p[2],g->p[2]); |
|
47 if (l > 0) { |
|
48 l = dRecipSqrt(l); |
|
49 g->p[0] = dMUL(g->p[0],l); |
|
50 g->p[1] = dMUL(g->p[1],l); |
|
51 g->p[2] = dMUL(g->p[2],l); |
|
52 g->p[3] = dMUL(g->p[3],l); |
|
53 } |
|
54 else { |
|
55 g->p[0] = REAL(1.0); |
|
56 g->p[1] = 0; |
|
57 g->p[2] = 0; |
|
58 g->p[3] = 0; |
|
59 } |
|
60 } |
|
61 |
|
62 |
|
63 dxPlane::dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d) : |
|
64 dxGeom (space,0) |
|
65 { |
|
66 type = dPlaneClass; |
|
67 p[0] = a; |
|
68 p[1] = b; |
|
69 p[2] = c; |
|
70 p[3] = d; |
|
71 make_sure_plane_normal_has_unit_length (this); |
|
72 } |
|
73 |
|
74 |
|
75 void dxPlane::computeAABB() |
|
76 { |
|
77 aabb[0] = -dInfinity; |
|
78 aabb[1] = dInfinity; |
|
79 aabb[2] = -dInfinity; |
|
80 aabb[3] = dInfinity; |
|
81 aabb[4] = -dInfinity; |
|
82 aabb[5] = dInfinity; |
|
83 |
|
84 // Planes that have normal vectors aligned along an axis can use a |
|
85 // less comprehensive (half space) bounding box. |
|
86 |
|
87 if ( p[1] == REAL(0.0f) && p[2] == REAL(0.0f) ) { |
|
88 // normal aligned with x-axis |
|
89 aabb[0] = (p[0] > 0) ? -dInfinity : -p[3]; |
|
90 aabb[1] = (p[0] > 0) ? p[3] : dInfinity; |
|
91 } else |
|
92 if ( p[0] == REAL(0.0f) && p[2] == REAL(0.0f) ) { |
|
93 // normal aligned with y-axis |
|
94 aabb[2] = (p[1] > 0) ? -dInfinity : -p[3]; |
|
95 aabb[3] = (p[1] > 0) ? p[3] : dInfinity; |
|
96 } else |
|
97 if ( p[0] == REAL(0.0f) && p[1] == REAL(0.0f) ) { |
|
98 // normal aligned with z-axis |
|
99 aabb[4] = (p[2] > 0) ? -dInfinity : -p[3]; |
|
100 aabb[5] = (p[2] > 0) ? p[3] : dInfinity; |
|
101 } |
|
102 } |
|
103 |
|
104 |
|
105 EXPORT_C dGeomID dCreatePlane (dSpaceID space, |
|
106 dReal a, dReal b, dReal c, dReal d) |
|
107 { |
|
108 return new dxPlane (space,a,b,c,d); |
|
109 } |
|
110 |
|
111 |
|
112 EXPORT_C void dGeomPlaneSetParams (dGeomID g, dReal a, dReal b, dReal c, dReal d) |
|
113 { |
|
114 dxPlane *p = (dxPlane*) g; |
|
115 p->p[0] = a; |
|
116 p->p[1] = b; |
|
117 p->p[2] = c; |
|
118 p->p[3] = d; |
|
119 make_sure_plane_normal_has_unit_length (p); |
|
120 dGeomMoved (g); |
|
121 } |
|
122 |
|
123 |
|
124 EXPORT_C void dGeomPlaneGetParams (dGeomID g, dVector4 result) |
|
125 { |
|
126 dxPlane *p = (dxPlane*) g; |
|
127 result[0] = p->p[0]; |
|
128 result[1] = p->p[1]; |
|
129 result[2] = p->p[2]; |
|
130 result[3] = p->p[3]; |
|
131 } |
|
132 |
|
133 |
|
134 EXPORT_C dReal dGeomPlanePointDepth (dGeomID g, dReal x, dReal y, dReal z) |
|
135 { |
|
136 dxPlane *p = (dxPlane*) g; |
|
137 return p->p[3] - dMUL(p->p[0],x) - dMUL(p->p[1],y) - dMUL(p->p[2],z); |
|
138 } |