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 | #ifdef _MSC_VER |
---|
42 | #pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" |
---|
43 | #endif |
---|
44 | |
---|
45 | //**************************************************************************** |
---|
46 | // plane public API |
---|
47 | |
---|
48 | static void make_sure_plane_normal_has_unit_length (dxPlane *g) |
---|
49 | { |
---|
50 | dReal l = g->p[0]*g->p[0] + g->p[1]*g->p[1] + g->p[2]*g->p[2]; |
---|
51 | if (l > 0) { |
---|
52 | l = dRecipSqrt(l); |
---|
53 | g->p[0] *= l; |
---|
54 | g->p[1] *= l; |
---|
55 | g->p[2] *= l; |
---|
56 | g->p[3] *= l; |
---|
57 | } |
---|
58 | else { |
---|
59 | g->p[0] = 1; |
---|
60 | g->p[1] = 0; |
---|
61 | g->p[2] = 0; |
---|
62 | g->p[3] = 0; |
---|
63 | } |
---|
64 | } |
---|
65 | |
---|
66 | |
---|
67 | dxPlane::dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d) : |
---|
68 | dxGeom (space,0) |
---|
69 | { |
---|
70 | type = dPlaneClass; |
---|
71 | p[0] = a; |
---|
72 | p[1] = b; |
---|
73 | p[2] = c; |
---|
74 | p[3] = d; |
---|
75 | make_sure_plane_normal_has_unit_length (this); |
---|
76 | } |
---|
77 | |
---|
78 | |
---|
79 | void dxPlane::computeAABB() |
---|
80 | { |
---|
81 | aabb[0] = -dInfinity; |
---|
82 | aabb[1] = dInfinity; |
---|
83 | aabb[2] = -dInfinity; |
---|
84 | aabb[3] = dInfinity; |
---|
85 | aabb[4] = -dInfinity; |
---|
86 | aabb[5] = dInfinity; |
---|
87 | |
---|
88 | // Planes that have normal vectors aligned along an axis can use a |
---|
89 | // less comprehensive (half space) bounding box. |
---|
90 | |
---|
91 | if ( p[1] == 0.0f && p[2] == 0.0f ) { |
---|
92 | // normal aligned with x-axis |
---|
93 | aabb[0] = (p[0] > 0) ? -dInfinity : -p[3]; |
---|
94 | aabb[1] = (p[0] > 0) ? p[3] : dInfinity; |
---|
95 | } else |
---|
96 | if ( p[0] == 0.0f && p[2] == 0.0f ) { |
---|
97 | // normal aligned with y-axis |
---|
98 | aabb[2] = (p[1] > 0) ? -dInfinity : -p[3]; |
---|
99 | aabb[3] = (p[1] > 0) ? p[3] : dInfinity; |
---|
100 | } else |
---|
101 | if ( p[0] == 0.0f && p[1] == 0.0f ) { |
---|
102 | // normal aligned with z-axis |
---|
103 | aabb[4] = (p[2] > 0) ? -dInfinity : -p[3]; |
---|
104 | aabb[5] = (p[2] > 0) ? p[3] : dInfinity; |
---|
105 | } |
---|
106 | } |
---|
107 | |
---|
108 | |
---|
109 | dGeomID dCreatePlane (dSpaceID space, |
---|
110 | dReal a, dReal b, dReal c, dReal d) |
---|
111 | { |
---|
112 | return new dxPlane (space,a,b,c,d); |
---|
113 | } |
---|
114 | |
---|
115 | |
---|
116 | void dGeomPlaneSetParams (dGeomID g, dReal a, dReal b, dReal c, dReal d) |
---|
117 | { |
---|
118 | dUASSERT (g && g->type == dPlaneClass,"argument not a plane"); |
---|
119 | dxPlane *p = (dxPlane*) g; |
---|
120 | p->p[0] = a; |
---|
121 | p->p[1] = b; |
---|
122 | p->p[2] = c; |
---|
123 | p->p[3] = d; |
---|
124 | make_sure_plane_normal_has_unit_length (p); |
---|
125 | dGeomMoved (g); |
---|
126 | } |
---|
127 | |
---|
128 | |
---|
129 | void dGeomPlaneGetParams (dGeomID g, dVector4 result) |
---|
130 | { |
---|
131 | dUASSERT (g && g->type == dPlaneClass,"argument not a plane"); |
---|
132 | dxPlane *p = (dxPlane*) g; |
---|
133 | result[0] = p->p[0]; |
---|
134 | result[1] = p->p[1]; |
---|
135 | result[2] = p->p[2]; |
---|
136 | result[3] = p->p[3]; |
---|
137 | } |
---|
138 | |
---|
139 | |
---|
140 | dReal dGeomPlanePointDepth (dGeomID g, dReal x, dReal y, dReal z) |
---|
141 | { |
---|
142 | dUASSERT (g && g->type == dPlaneClass,"argument not a plane"); |
---|
143 | dxPlane *p = (dxPlane*) g; |
---|
144 | return p->p[3] - p->p[0]*x - p->p[1]*y - p->p[2]*z; |
---|
145 | } |
---|