aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/irrlicht-1.8/source/Irrlicht/CSceneCollisionManager.cpp
diff options
context:
space:
mode:
authorDavid Walter Seikel2013-01-13 18:54:10 +1000
committerDavid Walter Seikel2013-01-13 18:54:10 +1000
commit959831f4ef5a3e797f576c3de08cd65032c997ad (patch)
treee7351908be5995f0b325b2ebeaa02d5a34b82583 /libraries/irrlicht-1.8/source/Irrlicht/CSceneCollisionManager.cpp
parentAdd info about changes to Irrlicht. (diff)
downloadSledjHamr-959831f4ef5a3e797f576c3de08cd65032c997ad.zip
SledjHamr-959831f4ef5a3e797f576c3de08cd65032c997ad.tar.gz
SledjHamr-959831f4ef5a3e797f576c3de08cd65032c997ad.tar.bz2
SledjHamr-959831f4ef5a3e797f576c3de08cd65032c997ad.tar.xz
Remove damned ancient DOS line endings from Irrlicht. Hopefully I did not go overboard.
Diffstat (limited to 'libraries/irrlicht-1.8/source/Irrlicht/CSceneCollisionManager.cpp')
-rw-r--r--libraries/irrlicht-1.8/source/Irrlicht/CSceneCollisionManager.cpp1920
1 files changed, 960 insertions, 960 deletions
diff --git a/libraries/irrlicht-1.8/source/Irrlicht/CSceneCollisionManager.cpp b/libraries/irrlicht-1.8/source/Irrlicht/CSceneCollisionManager.cpp
index cda4133..b3895bc 100644
--- a/libraries/irrlicht-1.8/source/Irrlicht/CSceneCollisionManager.cpp
+++ b/libraries/irrlicht-1.8/source/Irrlicht/CSceneCollisionManager.cpp
@@ -1,960 +1,960 @@
1// Copyright (C) 2002-2012 Nikolaus Gebhardt 1// Copyright (C) 2002-2012 Nikolaus Gebhardt
2// This file is part of the "Irrlicht Engine". 2// This file is part of the "Irrlicht Engine".
3// For conditions of distribution and use, see copyright notice in irrlicht.h 3// For conditions of distribution and use, see copyright notice in irrlicht.h
4 4
5#include "CSceneCollisionManager.h" 5#include "CSceneCollisionManager.h"
6#include "ISceneNode.h" 6#include "ISceneNode.h"
7#include "ICameraSceneNode.h" 7#include "ICameraSceneNode.h"
8#include "ITriangleSelector.h" 8#include "ITriangleSelector.h"
9#include "SViewFrustum.h" 9#include "SViewFrustum.h"
10 10
11#include "os.h" 11#include "os.h"
12#include "irrMath.h" 12#include "irrMath.h"
13 13
14namespace irr 14namespace irr
15{ 15{
16namespace scene 16namespace scene
17{ 17{
18 18
19//! constructor 19//! constructor
20CSceneCollisionManager::CSceneCollisionManager(ISceneManager* smanager, video::IVideoDriver* driver) 20CSceneCollisionManager::CSceneCollisionManager(ISceneManager* smanager, video::IVideoDriver* driver)
21: SceneManager(smanager), Driver(driver) 21: SceneManager(smanager), Driver(driver)
22{ 22{
23 #ifdef _DEBUG 23 #ifdef _DEBUG
24 setDebugName("CSceneCollisionManager"); 24 setDebugName("CSceneCollisionManager");
25 #endif 25 #endif
26 26
27 if (Driver) 27 if (Driver)
28 Driver->grab(); 28 Driver->grab();
29} 29}
30 30
31 31
32//! destructor 32//! destructor
33CSceneCollisionManager::~CSceneCollisionManager() 33CSceneCollisionManager::~CSceneCollisionManager()
34{ 34{
35 if (Driver) 35 if (Driver)
36 Driver->drop(); 36 Driver->drop();
37} 37}
38 38
39 39
40//! Returns the scene node, which is currently visible at the given 40//! Returns the scene node, which is currently visible at the given
41//! screen coordinates, viewed from the currently active camera. 41//! screen coordinates, viewed from the currently active camera.
42ISceneNode* CSceneCollisionManager::getSceneNodeFromScreenCoordinatesBB( 42ISceneNode* CSceneCollisionManager::getSceneNodeFromScreenCoordinatesBB(
43 const core::position2d<s32>& pos, s32 idBitMask, bool noDebugObjects, scene::ISceneNode* root) 43 const core::position2d<s32>& pos, s32 idBitMask, bool noDebugObjects, scene::ISceneNode* root)
44{ 44{
45 const core::line3d<f32> ln = getRayFromScreenCoordinates(pos, 0); 45 const core::line3d<f32> ln = getRayFromScreenCoordinates(pos, 0);
46 46
47 if ( ln.start == ln.end ) 47 if ( ln.start == ln.end )
48 return 0; 48 return 0;
49 49
50 return getSceneNodeFromRayBB(ln, idBitMask, noDebugObjects, root); 50 return getSceneNodeFromRayBB(ln, idBitMask, noDebugObjects, root);
51} 51}
52 52
53 53
54//! Returns the nearest scene node which collides with a 3d ray and 54//! Returns the nearest scene node which collides with a 3d ray and
55//! which id matches a bitmask. 55//! which id matches a bitmask.
56ISceneNode* CSceneCollisionManager::getSceneNodeFromRayBB( 56ISceneNode* CSceneCollisionManager::getSceneNodeFromRayBB(
57 const core::line3d<f32>& ray, 57 const core::line3d<f32>& ray,
58 s32 idBitMask, bool noDebugObjects, scene::ISceneNode* root) 58 s32 idBitMask, bool noDebugObjects, scene::ISceneNode* root)
59{ 59{
60 ISceneNode* best = 0; 60 ISceneNode* best = 0;
61 f32 dist = FLT_MAX; 61 f32 dist = FLT_MAX;
62 62
63 core::line3d<f32> truncatableRay(ray); 63 core::line3d<f32> truncatableRay(ray);
64 64
65 getPickedNodeBB((root==0)?SceneManager->getRootSceneNode():root, truncatableRay, 65 getPickedNodeBB((root==0)?SceneManager->getRootSceneNode():root, truncatableRay,
66 idBitMask, noDebugObjects, dist, best); 66 idBitMask, noDebugObjects, dist, best);
67 67
68 return best; 68 return best;
69} 69}
70 70
71 71
72//! recursive method for going through all scene nodes 72//! recursive method for going through all scene nodes
73void CSceneCollisionManager::getPickedNodeBB(ISceneNode* root, 73void CSceneCollisionManager::getPickedNodeBB(ISceneNode* root,
74 core::line3df& ray, s32 bits, bool noDebugObjects, 74 core::line3df& ray, s32 bits, bool noDebugObjects,
75 f32& outbestdistance, ISceneNode*& outbestnode) 75 f32& outbestdistance, ISceneNode*& outbestnode)
76{ 76{
77 const ISceneNodeList& children = root->getChildren(); 77 const ISceneNodeList& children = root->getChildren();
78 const core::vector3df rayVector = ray.getVector().normalize(); 78 const core::vector3df rayVector = ray.getVector().normalize();
79 79
80 ISceneNodeList::ConstIterator it = children.begin(); 80 ISceneNodeList::ConstIterator it = children.begin();
81 for (; it != children.end(); ++it) 81 for (; it != children.end(); ++it)
82 { 82 {
83 ISceneNode* current = *it; 83 ISceneNode* current = *it;
84 84
85 if (current->isVisible()) 85 if (current->isVisible())
86 { 86 {
87 if((noDebugObjects ? !current->isDebugObject() : true) && 87 if((noDebugObjects ? !current->isDebugObject() : true) &&
88 (bits==0 || (bits != 0 && (current->getID() & bits)))) 88 (bits==0 || (bits != 0 && (current->getID() & bits))))
89 { 89 {
90 // get world to object space transform 90 // get world to object space transform
91 core::matrix4 worldToObject; 91 core::matrix4 worldToObject;
92 if (!current->getAbsoluteTransformation().getInverse(worldToObject)) 92 if (!current->getAbsoluteTransformation().getInverse(worldToObject))
93 continue; 93 continue;
94 94
95 // transform vector from world space to object space 95 // transform vector from world space to object space
96 core::line3df objectRay(ray); 96 core::line3df objectRay(ray);
97 worldToObject.transformVect(objectRay.start); 97 worldToObject.transformVect(objectRay.start);
98 worldToObject.transformVect(objectRay.end); 98 worldToObject.transformVect(objectRay.end);
99 99
100 const core::aabbox3df & objectBox = current->getBoundingBox(); 100 const core::aabbox3df & objectBox = current->getBoundingBox();
101 101
102 // Do the initial intersection test in object space, since the 102 // Do the initial intersection test in object space, since the
103 // object space box test is more accurate. 103 // object space box test is more accurate.
104 if(objectBox.isPointInside(objectRay.start)) 104 if(objectBox.isPointInside(objectRay.start))
105 { 105 {
106 // use fast bbox intersection to find distance to hitpoint 106 // use fast bbox intersection to find distance to hitpoint
107 // algorithm from Kay et al., code from gamedev.net 107 // algorithm from Kay et al., code from gamedev.net
108 const core::vector3df dir = (objectRay.end-objectRay.start).normalize(); 108 const core::vector3df dir = (objectRay.end-objectRay.start).normalize();
109 const core::vector3df minDist = (objectBox.MinEdge - objectRay.start)/dir; 109 const core::vector3df minDist = (objectBox.MinEdge - objectRay.start)/dir;
110 const core::vector3df maxDist = (objectBox.MaxEdge - objectRay.start)/dir; 110 const core::vector3df maxDist = (objectBox.MaxEdge - objectRay.start)/dir;
111 const core::vector3df realMin(core::min_(minDist.X, maxDist.X),core::min_(minDist.Y, maxDist.Y),core::min_(minDist.Z, maxDist.Z)); 111 const core::vector3df realMin(core::min_(minDist.X, maxDist.X),core::min_(minDist.Y, maxDist.Y),core::min_(minDist.Z, maxDist.Z));
112 const core::vector3df realMax(core::max_(minDist.X, maxDist.X),core::max_(minDist.Y, maxDist.Y),core::max_(minDist.Z, maxDist.Z)); 112 const core::vector3df realMax(core::max_(minDist.X, maxDist.X),core::max_(minDist.Y, maxDist.Y),core::max_(minDist.Z, maxDist.Z));
113 113
114 const f32 minmax = core::min_(realMax.X, realMax.Y, realMax.Z); 114 const f32 minmax = core::min_(realMax.X, realMax.Y, realMax.Z);
115 // nearest distance to intersection 115 // nearest distance to intersection
116 const f32 maxmin = core::max_(realMin.X, realMin.Y, realMin.Z); 116 const f32 maxmin = core::max_(realMin.X, realMin.Y, realMin.Z);
117 117
118 const f32 toIntersectionSq = (maxmin>0?maxmin*maxmin:minmax*minmax); 118 const f32 toIntersectionSq = (maxmin>0?maxmin*maxmin:minmax*minmax);
119 if (toIntersectionSq < outbestdistance) 119 if (toIntersectionSq < outbestdistance)
120 { 120 {
121 outbestdistance = toIntersectionSq; 121 outbestdistance = toIntersectionSq;
122 outbestnode = current; 122 outbestnode = current;
123 123
124 // And we can truncate the ray to stop us hitting further nodes. 124 // And we can truncate the ray to stop us hitting further nodes.
125 ray.end = ray.start + (rayVector * sqrtf(toIntersectionSq)); 125 ray.end = ray.start + (rayVector * sqrtf(toIntersectionSq));
126 } 126 }
127 } 127 }
128 else 128 else
129 if (objectBox.intersectsWithLine(objectRay)) 129 if (objectBox.intersectsWithLine(objectRay))
130 { 130 {
131 // Now transform into world space, since we need to use world space 131 // Now transform into world space, since we need to use world space
132 // scales and distances. 132 // scales and distances.
133 core::aabbox3df worldBox(objectBox); 133 core::aabbox3df worldBox(objectBox);
134 current->getAbsoluteTransformation().transformBox(worldBox); 134 current->getAbsoluteTransformation().transformBox(worldBox);
135 135
136 core::vector3df edges[8]; 136 core::vector3df edges[8];
137 worldBox.getEdges(edges); 137 worldBox.getEdges(edges);
138 138
139 /* We need to check against each of 6 faces, composed of these corners: 139 /* We need to check against each of 6 faces, composed of these corners:
140 /3--------/7 140 /3--------/7
141 / | / | 141 / | / |
142 / | / | 142 / | / |
143 1---------5 | 143 1---------5 |
144 | 2- - -| -6 144 | 2- - -| -6
145 | / | / 145 | / | /
146 |/ | / 146 |/ | /
147 0---------4/ 147 0---------4/
148 148
149 Note that we define them as opposite pairs of faces. 149 Note that we define them as opposite pairs of faces.
150 */ 150 */
151 static const s32 faceEdges[6][3] = 151 static const s32 faceEdges[6][3] =
152 { 152 {
153 { 0, 1, 5 }, // Front 153 { 0, 1, 5 }, // Front
154 { 6, 7, 3 }, // Back 154 { 6, 7, 3 }, // Back
155 { 2, 3, 1 }, // Left 155 { 2, 3, 1 }, // Left
156 { 4, 5, 7 }, // Right 156 { 4, 5, 7 }, // Right
157 { 1, 3, 7 }, // Top 157 { 1, 3, 7 }, // Top
158 { 2, 0, 4 } // Bottom 158 { 2, 0, 4 } // Bottom
159 }; 159 };
160 160
161 core::vector3df intersection; 161 core::vector3df intersection;
162 core::plane3df facePlane; 162 core::plane3df facePlane;
163 f32 bestDistToBoxBorder = FLT_MAX; 163 f32 bestDistToBoxBorder = FLT_MAX;
164 f32 bestToIntersectionSq = FLT_MAX; 164 f32 bestToIntersectionSq = FLT_MAX;
165 165
166 for(s32 face = 0; face < 6; ++face) 166 for(s32 face = 0; face < 6; ++face)
167 { 167 {
168 facePlane.setPlane(edges[faceEdges[face][0]], 168 facePlane.setPlane(edges[faceEdges[face][0]],
169 edges[faceEdges[face][1]], 169 edges[faceEdges[face][1]],
170 edges[faceEdges[face][2]]); 170 edges[faceEdges[face][2]]);
171 171
172 // Only consider lines that might be entering through this face, since we 172 // Only consider lines that might be entering through this face, since we
173 // already know that the start point is outside the box. 173 // already know that the start point is outside the box.
174 if(facePlane.classifyPointRelation(ray.start) != core::ISREL3D_FRONT) 174 if(facePlane.classifyPointRelation(ray.start) != core::ISREL3D_FRONT)
175 continue; 175 continue;
176 176
177 // Don't bother using a limited ray, since we already know that it should be long 177 // Don't bother using a limited ray, since we already know that it should be long
178 // enough to intersect with the box. 178 // enough to intersect with the box.
179 if(facePlane.getIntersectionWithLine(ray.start, rayVector, intersection)) 179 if(facePlane.getIntersectionWithLine(ray.start, rayVector, intersection))
180 { 180 {
181 const f32 toIntersectionSq = ray.start.getDistanceFromSQ(intersection); 181 const f32 toIntersectionSq = ray.start.getDistanceFromSQ(intersection);
182 if(toIntersectionSq < outbestdistance) 182 if(toIntersectionSq < outbestdistance)
183 { 183 {
184 // We have to check that the intersection with this plane is actually 184 // We have to check that the intersection with this plane is actually
185 // on the box, so need to go back to object space again. 185 // on the box, so need to go back to object space again.
186 worldToObject.transformVect(intersection); 186 worldToObject.transformVect(intersection);
187 187
188 // find the closest point on the box borders. Have to do this as exact checks will fail due to floating point problems. 188 // find the closest point on the box borders. Have to do this as exact checks will fail due to floating point problems.
189 f32 distToBorder = core::max_ ( core::min_ (core::abs_(objectBox.MinEdge.X-intersection.X), core::abs_(objectBox.MaxEdge.X-intersection.X)), 189 f32 distToBorder = core::max_ ( core::min_ (core::abs_(objectBox.MinEdge.X-intersection.X), core::abs_(objectBox.MaxEdge.X-intersection.X)),
190 core::min_ (core::abs_(objectBox.MinEdge.Y-intersection.Y), core::abs_(objectBox.MaxEdge.Y-intersection.Y)), 190 core::min_ (core::abs_(objectBox.MinEdge.Y-intersection.Y), core::abs_(objectBox.MaxEdge.Y-intersection.Y)),
191 core::min_ (core::abs_(objectBox.MinEdge.Z-intersection.Z), core::abs_(objectBox.MaxEdge.Z-intersection.Z)) ); 191 core::min_ (core::abs_(objectBox.MinEdge.Z-intersection.Z), core::abs_(objectBox.MaxEdge.Z-intersection.Z)) );
192 if ( distToBorder < bestDistToBoxBorder ) 192 if ( distToBorder < bestDistToBoxBorder )
193 { 193 {
194 bestDistToBoxBorder = distToBorder; 194 bestDistToBoxBorder = distToBorder;
195 bestToIntersectionSq = toIntersectionSq; 195 bestToIntersectionSq = toIntersectionSq;
196 } 196 }
197 } 197 }
198 } 198 }
199 199
200 // If the ray could be entering through the first face of a pair, then it can't 200 // If the ray could be entering through the first face of a pair, then it can't
201 // also be entering through the opposite face, and so we can skip that face. 201 // also be entering through the opposite face, and so we can skip that face.
202 if (!(face & 0x01)) 202 if (!(face & 0x01))
203 ++face; 203 ++face;
204 } 204 }
205 205
206 if ( bestDistToBoxBorder < FLT_MAX ) 206 if ( bestDistToBoxBorder < FLT_MAX )
207 { 207 {
208 outbestdistance = bestToIntersectionSq; 208 outbestdistance = bestToIntersectionSq;
209 outbestnode = current; 209 outbestnode = current;
210 210
211 // If we got a hit, we can now truncate the ray to stop us hitting further nodes. 211 // If we got a hit, we can now truncate the ray to stop us hitting further nodes.
212 ray.end = ray.start + (rayVector * sqrtf(outbestdistance)); 212 ray.end = ray.start + (rayVector * sqrtf(outbestdistance));
213 } 213 }
214 } 214 }
215 } 215 }
216 216
217 // Only check the children if this node is visible. 217 // Only check the children if this node is visible.
218 getPickedNodeBB(current, ray, bits, noDebugObjects, outbestdistance, outbestnode); 218 getPickedNodeBB(current, ray, bits, noDebugObjects, outbestdistance, outbestnode);
219 } 219 }
220 } 220 }
221} 221}
222 222
223 223
224ISceneNode* CSceneCollisionManager::getSceneNodeAndCollisionPointFromRay( 224ISceneNode* CSceneCollisionManager::getSceneNodeAndCollisionPointFromRay(
225 core::line3df ray, 225 core::line3df ray,
226 core::vector3df & outCollisionPoint, 226 core::vector3df & outCollisionPoint,
227 core::triangle3df & outTriangle, 227 core::triangle3df & outTriangle,
228 s32 idBitMask, 228 s32 idBitMask,
229 ISceneNode * collisionRootNode, 229 ISceneNode * collisionRootNode,
230 bool noDebugObjects) 230 bool noDebugObjects)
231{ 231{
232 ISceneNode* bestNode = 0; 232 ISceneNode* bestNode = 0;
233 f32 bestDistanceSquared = FLT_MAX; 233 f32 bestDistanceSquared = FLT_MAX;
234 234
235 if(0 == collisionRootNode) 235 if(0 == collisionRootNode)
236 collisionRootNode = SceneManager->getRootSceneNode(); 236 collisionRootNode = SceneManager->getRootSceneNode();
237 237
238 // We don't try to do anything too clever, like sorting the candidate 238 // We don't try to do anything too clever, like sorting the candidate
239 // nodes by distance to bounding-box. In the example below, we could do the 239 // nodes by distance to bounding-box. In the example below, we could do the
240 // triangle collision check with node A first, but we'd have to check node B 240 // triangle collision check with node A first, but we'd have to check node B
241 // anyway, as the actual collision point could be (and is) closer than the 241 // anyway, as the actual collision point could be (and is) closer than the
242 // collision point in node A. 242 // collision point in node A.
243 // 243 //
244 // ray end 244 // ray end
245 // | 245 // |
246 // AAAAAAAAAA 246 // AAAAAAAAAA
247 // A | 247 // A |
248 // A | B 248 // A | B
249 // A | B 249 // A | B
250 // A BBBBB 250 // A BBBBB
251 // A | 251 // A |
252 // A | 252 // A |
253 // | 253 // |
254 // | 254 // |
255 // ray start 255 // ray start
256 // 256 //
257 // We therefore have to do a full BB and triangle collision on every scene 257 // We therefore have to do a full BB and triangle collision on every scene
258 // node in order to find the nearest collision point, so sorting them by 258 // node in order to find the nearest collision point, so sorting them by
259 // bounding box would be pointless. 259 // bounding box would be pointless.
260 260
261 getPickedNodeFromBBAndSelector(collisionRootNode, ray, idBitMask, 261 getPickedNodeFromBBAndSelector(collisionRootNode, ray, idBitMask,
262 noDebugObjects, bestDistanceSquared, bestNode, 262 noDebugObjects, bestDistanceSquared, bestNode,
263 outCollisionPoint, outTriangle); 263 outCollisionPoint, outTriangle);
264 return bestNode; 264 return bestNode;
265} 265}
266 266
267 267
268void CSceneCollisionManager::getPickedNodeFromBBAndSelector( 268void CSceneCollisionManager::getPickedNodeFromBBAndSelector(
269 ISceneNode * root, 269 ISceneNode * root,
270 core::line3df & ray, 270 core::line3df & ray,
271 s32 bits, 271 s32 bits,
272 bool noDebugObjects, 272 bool noDebugObjects,
273 f32 & outBestDistanceSquared, 273 f32 & outBestDistanceSquared,
274 ISceneNode * & outBestNode, 274 ISceneNode * & outBestNode,
275 core::vector3df & outBestCollisionPoint, 275 core::vector3df & outBestCollisionPoint,
276 core::triangle3df & outBestTriangle) 276 core::triangle3df & outBestTriangle)
277{ 277{
278 const ISceneNodeList& children = root->getChildren(); 278 const ISceneNodeList& children = root->getChildren();
279 279
280 ISceneNodeList::ConstIterator it = children.begin(); 280 ISceneNodeList::ConstIterator it = children.begin();
281 for (; it != children.end(); ++it) 281 for (; it != children.end(); ++it)
282 { 282 {
283 ISceneNode* current = *it; 283 ISceneNode* current = *it;
284 ITriangleSelector * selector = current->getTriangleSelector(); 284 ITriangleSelector * selector = current->getTriangleSelector();
285 285
286 if (selector && current->isVisible() && 286 if (selector && current->isVisible() &&
287 (noDebugObjects ? !current->isDebugObject() : true) && 287 (noDebugObjects ? !current->isDebugObject() : true) &&
288 (bits==0 || (bits != 0 && (current->getID() & bits)))) 288 (bits==0 || (bits != 0 && (current->getID() & bits))))
289 { 289 {
290 // get world to object space transform 290 // get world to object space transform
291 core::matrix4 mat; 291 core::matrix4 mat;
292 if (!current->getAbsoluteTransformation().getInverse(mat)) 292 if (!current->getAbsoluteTransformation().getInverse(mat))
293 continue; 293 continue;
294 294
295 // transform vector from world space to object space 295 // transform vector from world space to object space
296 core::line3df line(ray); 296 core::line3df line(ray);
297 mat.transformVect(line.start); 297 mat.transformVect(line.start);
298 mat.transformVect(line.end); 298 mat.transformVect(line.end);
299 299
300 const core::aabbox3df& box = current->getBoundingBox(); 300 const core::aabbox3df& box = current->getBoundingBox();
301 301
302 core::vector3df candidateCollisionPoint; 302 core::vector3df candidateCollisionPoint;
303 core::triangle3df candidateTriangle; 303 core::triangle3df candidateTriangle;
304 304
305 // do intersection test in object space 305 // do intersection test in object space
306 ISceneNode * hitNode = 0; 306 ISceneNode * hitNode = 0;
307 if (box.intersectsWithLine(line) && 307 if (box.intersectsWithLine(line) &&
308 getCollisionPoint(ray, selector, candidateCollisionPoint, candidateTriangle, hitNode)) 308 getCollisionPoint(ray, selector, candidateCollisionPoint, candidateTriangle, hitNode))
309 { 309 {
310 const f32 distanceSquared = (candidateCollisionPoint - ray.start).getLengthSQ(); 310 const f32 distanceSquared = (candidateCollisionPoint - ray.start).getLengthSQ();
311 311
312 if(distanceSquared < outBestDistanceSquared) 312 if(distanceSquared < outBestDistanceSquared)
313 { 313 {
314 outBestDistanceSquared = distanceSquared; 314 outBestDistanceSquared = distanceSquared;
315 outBestNode = current; 315 outBestNode = current;
316 outBestCollisionPoint = candidateCollisionPoint; 316 outBestCollisionPoint = candidateCollisionPoint;
317 outBestTriangle = candidateTriangle; 317 outBestTriangle = candidateTriangle;
318 const core::vector3df rayVector = ray.getVector().normalize(); 318 const core::vector3df rayVector = ray.getVector().normalize();
319 ray.end = ray.start + (rayVector * sqrtf(distanceSquared)); 319 ray.end = ray.start + (rayVector * sqrtf(distanceSquared));
320 } 320 }
321 } 321 }
322 } 322 }
323 323
324 getPickedNodeFromBBAndSelector(current, ray, bits, noDebugObjects, 324 getPickedNodeFromBBAndSelector(current, ray, bits, noDebugObjects,
325 outBestDistanceSquared, outBestNode, 325 outBestDistanceSquared, outBestNode,
326 outBestCollisionPoint, outBestTriangle); 326 outBestCollisionPoint, outBestTriangle);
327 } 327 }
328} 328}
329 329
330 330
331//! Returns the scene node, at which the overgiven camera is looking at and 331//! Returns the scene node, at which the overgiven camera is looking at and
332//! which id matches the bitmask. 332//! which id matches the bitmask.
333ISceneNode* CSceneCollisionManager::getSceneNodeFromCameraBB( 333ISceneNode* CSceneCollisionManager::getSceneNodeFromCameraBB(
334 ICameraSceneNode* camera, s32 idBitMask, bool noDebugObjects) 334 ICameraSceneNode* camera, s32 idBitMask, bool noDebugObjects)
335{ 335{
336 if (!camera) 336 if (!camera)
337 return 0; 337 return 0;
338 338
339 const core::vector3df start = camera->getAbsolutePosition(); 339 const core::vector3df start = camera->getAbsolutePosition();
340 core::vector3df end = camera->getTarget(); 340 core::vector3df end = camera->getTarget();
341 341
342 end = start + ((end - start).normalize() * camera->getFarValue()); 342 end = start + ((end - start).normalize() * camera->getFarValue());
343 343
344 return getSceneNodeFromRayBB(core::line3d<f32>(start, end), idBitMask, noDebugObjects); 344 return getSceneNodeFromRayBB(core::line3d<f32>(start, end), idBitMask, noDebugObjects);
345} 345}
346 346
347 347
348//! Finds the collision point of a line and lots of triangles, if there is one. 348//! Finds the collision point of a line and lots of triangles, if there is one.
349bool CSceneCollisionManager::getCollisionPoint(const core::line3d<f32>& ray, 349bool CSceneCollisionManager::getCollisionPoint(const core::line3d<f32>& ray,
350 ITriangleSelector* selector, core::vector3df& outIntersection, 350 ITriangleSelector* selector, core::vector3df& outIntersection,
351 core::triangle3df& outTriangle, 351 core::triangle3df& outTriangle,
352 ISceneNode*& outNode) 352 ISceneNode*& outNode)
353{ 353{
354 if (!selector) 354 if (!selector)
355 { 355 {
356 _IRR_IMPLEMENT_MANAGED_MARSHALLING_BUGFIX; 356 _IRR_IMPLEMENT_MANAGED_MARSHALLING_BUGFIX;
357 return false; 357 return false;
358 } 358 }
359 359
360 s32 totalcnt = selector->getTriangleCount(); 360 s32 totalcnt = selector->getTriangleCount();
361 if ( totalcnt <= 0 ) 361 if ( totalcnt <= 0 )
362 return false; 362 return false;
363 363
364 Triangles.set_used(totalcnt); 364 Triangles.set_used(totalcnt);
365 365
366 s32 cnt = 0; 366 s32 cnt = 0;
367 selector->getTriangles(Triangles.pointer(), totalcnt, cnt, ray); 367 selector->getTriangles(Triangles.pointer(), totalcnt, cnt, ray);
368 368
369 const core::vector3df linevect = ray.getVector().normalize(); 369 const core::vector3df linevect = ray.getVector().normalize();
370 core::vector3df intersection; 370 core::vector3df intersection;
371 f32 nearest = FLT_MAX; 371 f32 nearest = FLT_MAX;
372 bool found = false; 372 bool found = false;
373 const f32 raylength = ray.getLengthSQ(); 373 const f32 raylength = ray.getLengthSQ();
374 374
375 const f32 minX = core::min_(ray.start.X, ray.end.X); 375 const f32 minX = core::min_(ray.start.X, ray.end.X);
376 const f32 maxX = core::max_(ray.start.X, ray.end.X); 376 const f32 maxX = core::max_(ray.start.X, ray.end.X);
377 const f32 minY = core::min_(ray.start.Y, ray.end.Y); 377 const f32 minY = core::min_(ray.start.Y, ray.end.Y);
378 const f32 maxY = core::max_(ray.start.Y, ray.end.Y); 378 const f32 maxY = core::max_(ray.start.Y, ray.end.Y);
379 const f32 minZ = core::min_(ray.start.Z, ray.end.Z); 379 const f32 minZ = core::min_(ray.start.Z, ray.end.Z);
380 const f32 maxZ = core::max_(ray.start.Z, ray.end.Z); 380 const f32 maxZ = core::max_(ray.start.Z, ray.end.Z);
381 381
382 for (s32 i=0; i<cnt; ++i) 382 for (s32 i=0; i<cnt; ++i)
383 { 383 {
384 const core::triangle3df & triangle = Triangles[i]; 384 const core::triangle3df & triangle = Triangles[i];
385 385
386 if(minX > triangle.pointA.X && minX > triangle.pointB.X && minX > triangle.pointC.X) 386 if(minX > triangle.pointA.X && minX > triangle.pointB.X && minX > triangle.pointC.X)
387 continue; 387 continue;
388 if(maxX < triangle.pointA.X && maxX < triangle.pointB.X && maxX < triangle.pointC.X) 388 if(maxX < triangle.pointA.X && maxX < triangle.pointB.X && maxX < triangle.pointC.X)
389 continue; 389 continue;
390 if(minY > triangle.pointA.Y && minY > triangle.pointB.Y && minY > triangle.pointC.Y) 390 if(minY > triangle.pointA.Y && minY > triangle.pointB.Y && minY > triangle.pointC.Y)
391 continue; 391 continue;
392 if(maxY < triangle.pointA.Y && maxY < triangle.pointB.Y && maxY < triangle.pointC.Y) 392 if(maxY < triangle.pointA.Y && maxY < triangle.pointB.Y && maxY < triangle.pointC.Y)
393 continue; 393 continue;
394 if(minZ > triangle.pointA.Z && minZ > triangle.pointB.Z && minZ > triangle.pointC.Z) 394 if(minZ > triangle.pointA.Z && minZ > triangle.pointB.Z && minZ > triangle.pointC.Z)
395 continue; 395 continue;
396 if(maxZ < triangle.pointA.Z && maxZ < triangle.pointB.Z && maxZ < triangle.pointC.Z) 396 if(maxZ < triangle.pointA.Z && maxZ < triangle.pointB.Z && maxZ < triangle.pointC.Z)
397 continue; 397 continue;
398 398
399 if (triangle.getIntersectionWithLine(ray.start, linevect, intersection)) 399 if (triangle.getIntersectionWithLine(ray.start, linevect, intersection))
400 { 400 {
401 const f32 tmp = intersection.getDistanceFromSQ(ray.start); 401 const f32 tmp = intersection.getDistanceFromSQ(ray.start);
402 const f32 tmp2 = intersection.getDistanceFromSQ(ray.end); 402 const f32 tmp2 = intersection.getDistanceFromSQ(ray.end);
403 403
404 if (tmp < raylength && tmp2 < raylength && tmp < nearest) 404 if (tmp < raylength && tmp2 < raylength && tmp < nearest)
405 { 405 {
406 nearest = tmp; 406 nearest = tmp;
407 outTriangle = triangle; 407 outTriangle = triangle;
408 outIntersection = intersection; 408 outIntersection = intersection;
409 outNode = selector->getSceneNodeForTriangle(i); 409 outNode = selector->getSceneNodeForTriangle(i);
410 found = true; 410 found = true;
411 } 411 }
412 } 412 }
413 } 413 }
414 414
415 _IRR_IMPLEMENT_MANAGED_MARSHALLING_BUGFIX; 415 _IRR_IMPLEMENT_MANAGED_MARSHALLING_BUGFIX;
416 return found; 416 return found;
417} 417}
418 418
419 419
420//! Collides a moving ellipsoid with a 3d world with gravity and returns 420//! Collides a moving ellipsoid with a 3d world with gravity and returns
421//! the resulting new position of the ellipsoid. 421//! the resulting new position of the ellipsoid.
422core::vector3df CSceneCollisionManager::getCollisionResultPosition( 422core::vector3df CSceneCollisionManager::getCollisionResultPosition(
423 ITriangleSelector* selector, 423 ITriangleSelector* selector,
424 const core::vector3df &position, const core::vector3df& radius, 424 const core::vector3df &position, const core::vector3df& radius,
425 const core::vector3df& direction, 425 const core::vector3df& direction,
426 core::triangle3df& triout, 426 core::triangle3df& triout,
427 core::vector3df& hitPosition, 427 core::vector3df& hitPosition,
428 bool& outFalling, 428 bool& outFalling,
429 ISceneNode*& outNode, 429 ISceneNode*& outNode,
430 f32 slidingSpeed, 430 f32 slidingSpeed,
431 const core::vector3df& gravity) 431 const core::vector3df& gravity)
432{ 432{
433 return collideEllipsoidWithWorld(selector, position, 433 return collideEllipsoidWithWorld(selector, position,
434 radius, direction, slidingSpeed, gravity, triout, hitPosition, outFalling, outNode); 434 radius, direction, slidingSpeed, gravity, triout, hitPosition, outFalling, outNode);
435} 435}
436 436
437 437
438bool CSceneCollisionManager::testTriangleIntersection(SCollisionData* colData, 438bool CSceneCollisionManager::testTriangleIntersection(SCollisionData* colData,
439 const core::triangle3df& triangle) 439 const core::triangle3df& triangle)
440{ 440{
441 const core::plane3d<f32> trianglePlane = triangle.getPlane(); 441 const core::plane3d<f32> trianglePlane = triangle.getPlane();
442 442
443 // only check front facing polygons 443 // only check front facing polygons
444 if ( !trianglePlane.isFrontFacing(colData->normalizedVelocity) ) 444 if ( !trianglePlane.isFrontFacing(colData->normalizedVelocity) )
445 return false; 445 return false;
446 446
447 // get interval of plane intersection 447 // get interval of plane intersection
448 448
449 f32 t1, t0; 449 f32 t1, t0;
450 bool embeddedInPlane = false; 450 bool embeddedInPlane = false;
451 451
452 // calculate signed distance from sphere position to triangle plane 452 // calculate signed distance from sphere position to triangle plane
453 f32 signedDistToTrianglePlane = trianglePlane.getDistanceTo( 453 f32 signedDistToTrianglePlane = trianglePlane.getDistanceTo(
454 colData->basePoint); 454 colData->basePoint);
455 455
456 f32 normalDotVelocity = 456 f32 normalDotVelocity =
457 trianglePlane.Normal.dotProduct(colData->velocity); 457 trianglePlane.Normal.dotProduct(colData->velocity);
458 458
459 if ( core::iszero ( normalDotVelocity ) ) 459 if ( core::iszero ( normalDotVelocity ) )
460 { 460 {
461 // sphere is traveling parallel to plane 461 // sphere is traveling parallel to plane
462 462
463 if (fabs(signedDistToTrianglePlane) >= 1.0f) 463 if (fabs(signedDistToTrianglePlane) >= 1.0f)
464 return false; // no collision possible 464 return false; // no collision possible
465 else 465 else
466 { 466 {
467 // sphere is embedded in plane 467 // sphere is embedded in plane
468 embeddedInPlane = true; 468 embeddedInPlane = true;
469 t0 = 0.0; 469 t0 = 0.0;
470 t1 = 1.0; 470 t1 = 1.0;
471 } 471 }
472 } 472 }
473 else 473 else
474 { 474 {
475 normalDotVelocity = core::reciprocal ( normalDotVelocity ); 475 normalDotVelocity = core::reciprocal ( normalDotVelocity );
476 476
477 // N.D is not 0. Calculate intersection interval 477 // N.D is not 0. Calculate intersection interval
478 t0 = (-1.f - signedDistToTrianglePlane) * normalDotVelocity; 478 t0 = (-1.f - signedDistToTrianglePlane) * normalDotVelocity;
479 t1 = (1.f - signedDistToTrianglePlane) * normalDotVelocity; 479 t1 = (1.f - signedDistToTrianglePlane) * normalDotVelocity;
480 480
481 // Swap so t0 < t1 481 // Swap so t0 < t1
482 if (t0 > t1) { f32 tmp = t1; t1 = t0; t0 = tmp; } 482 if (t0 > t1) { f32 tmp = t1; t1 = t0; t0 = tmp; }
483 483
484 // check if at least one value is within the range 484 // check if at least one value is within the range
485 if (t0 > 1.0f || t1 < 0.0f) 485 if (t0 > 1.0f || t1 < 0.0f)
486 return false; // both t values are outside 1 and 0, no collision possible 486 return false; // both t values are outside 1 and 0, no collision possible
487 487
488 // clamp to 0 and 1 488 // clamp to 0 and 1
489 t0 = core::clamp ( t0, 0.f, 1.f ); 489 t0 = core::clamp ( t0, 0.f, 1.f );
490 t1 = core::clamp ( t1, 0.f, 1.f ); 490 t1 = core::clamp ( t1, 0.f, 1.f );
491 } 491 }
492 492
493 // at this point we have t0 and t1, if there is any intersection, it 493 // at this point we have t0 and t1, if there is any intersection, it
494 // is between this interval 494 // is between this interval
495 core::vector3df collisionPoint; 495 core::vector3df collisionPoint;
496 bool foundCollision = false; 496 bool foundCollision = false;
497 f32 t = 1.0f; 497 f32 t = 1.0f;
498 498
499 // first check the easy case: Collision within the triangle; 499 // first check the easy case: Collision within the triangle;
500 // if this happens, it must be at t0 and this is when the sphere 500 // if this happens, it must be at t0 and this is when the sphere
501 // rests on the front side of the triangle plane. This can only happen 501 // rests on the front side of the triangle plane. This can only happen
502 // if the sphere is not embedded in the triangle plane. 502 // if the sphere is not embedded in the triangle plane.
503 503
504 if (!embeddedInPlane) 504 if (!embeddedInPlane)
505 { 505 {
506 core::vector3df planeIntersectionPoint = 506 core::vector3df planeIntersectionPoint =
507 (colData->basePoint - trianglePlane.Normal) 507 (colData->basePoint - trianglePlane.Normal)
508 + (colData->velocity * t0); 508 + (colData->velocity * t0);
509 509
510 if (triangle.isPointInside(planeIntersectionPoint)) 510 if (triangle.isPointInside(planeIntersectionPoint))
511 { 511 {
512 foundCollision = true; 512 foundCollision = true;
513 t = t0; 513 t = t0;
514 collisionPoint = planeIntersectionPoint; 514 collisionPoint = planeIntersectionPoint;
515 } 515 }
516 } 516 }
517 517
518 // if we havent found a collision already we will have to sweep 518 // if we havent found a collision already we will have to sweep
519 // the sphere against points and edges of the triangle. Note: A 519 // the sphere against points and edges of the triangle. Note: A
520 // collision inside the triangle will always happen before a 520 // collision inside the triangle will always happen before a
521 // vertex or edge collision. 521 // vertex or edge collision.
522 522
523 if (!foundCollision) 523 if (!foundCollision)
524 { 524 {
525 core::vector3df velocity = colData->velocity; 525 core::vector3df velocity = colData->velocity;
526 core::vector3df base = colData->basePoint; 526 core::vector3df base = colData->basePoint;
527 527
528 f32 velocitySqaredLength = velocity.getLengthSQ(); 528 f32 velocitySqaredLength = velocity.getLengthSQ();
529 f32 a,b,c; 529 f32 a,b,c;
530 f32 newT; 530 f32 newT;
531 531
532 // for each edge or vertex a quadratic equation has to be solved: 532 // for each edge or vertex a quadratic equation has to be solved:
533 // a*t^2 + b*t + c = 0. We calculate a,b, and c for each test. 533 // a*t^2 + b*t + c = 0. We calculate a,b, and c for each test.
534 534
535 // check against points 535 // check against points
536 a = velocitySqaredLength; 536 a = velocitySqaredLength;
537 537
538 // p1 538 // p1
539 b = 2.0f * (velocity.dotProduct(base - triangle.pointA)); 539 b = 2.0f * (velocity.dotProduct(base - triangle.pointA));
540 c = (triangle.pointA-base).getLengthSQ() - 1.f; 540 c = (triangle.pointA-base).getLengthSQ() - 1.f;
541 if (getLowestRoot(a,b,c,t, &newT)) 541 if (getLowestRoot(a,b,c,t, &newT))
542 { 542 {
543 t = newT; 543 t = newT;
544 foundCollision = true; 544 foundCollision = true;
545 collisionPoint = triangle.pointA; 545 collisionPoint = triangle.pointA;
546 } 546 }
547 547
548 // p2 548 // p2
549 if (!foundCollision) 549 if (!foundCollision)
550 { 550 {
551 b = 2.0f * (velocity.dotProduct(base - triangle.pointB)); 551 b = 2.0f * (velocity.dotProduct(base - triangle.pointB));
552 c = (triangle.pointB-base).getLengthSQ() - 1.f; 552 c = (triangle.pointB-base).getLengthSQ() - 1.f;
553 if (getLowestRoot(a,b,c,t, &newT)) 553 if (getLowestRoot(a,b,c,t, &newT))
554 { 554 {
555 t = newT; 555 t = newT;
556 foundCollision = true; 556 foundCollision = true;
557 collisionPoint = triangle.pointB; 557 collisionPoint = triangle.pointB;
558 } 558 }
559 } 559 }
560 560
561 // p3 561 // p3
562 if (!foundCollision) 562 if (!foundCollision)
563 { 563 {
564 b = 2.0f * (velocity.dotProduct(base - triangle.pointC)); 564 b = 2.0f * (velocity.dotProduct(base - triangle.pointC));
565 c = (triangle.pointC-base).getLengthSQ() - 1.f; 565 c = (triangle.pointC-base).getLengthSQ() - 1.f;
566 if (getLowestRoot(a,b,c,t, &newT)) 566 if (getLowestRoot(a,b,c,t, &newT))
567 { 567 {
568 t = newT; 568 t = newT;
569 foundCollision = true; 569 foundCollision = true;
570 collisionPoint = triangle.pointC; 570 collisionPoint = triangle.pointC;
571 } 571 }
572 } 572 }
573 573
574 // check against edges: 574 // check against edges:
575 575
576 // p1 --- p2 576 // p1 --- p2
577 core::vector3df edge = triangle.pointB - triangle.pointA; 577 core::vector3df edge = triangle.pointB - triangle.pointA;
578 core::vector3df baseToVertex = triangle.pointA - base; 578 core::vector3df baseToVertex = triangle.pointA - base;
579 f32 edgeSqaredLength = edge.getLengthSQ(); 579 f32 edgeSqaredLength = edge.getLengthSQ();
580 f32 edgeDotVelocity = edge.dotProduct(velocity); 580 f32 edgeDotVelocity = edge.dotProduct(velocity);
581 f32 edgeDotBaseToVertex = edge.dotProduct(baseToVertex); 581 f32 edgeDotBaseToVertex = edge.dotProduct(baseToVertex);
582 582
583 // calculate parameters for equation 583 // calculate parameters for equation
584 a = edgeSqaredLength* -velocitySqaredLength + 584 a = edgeSqaredLength* -velocitySqaredLength +
585 edgeDotVelocity*edgeDotVelocity; 585 edgeDotVelocity*edgeDotVelocity;
586 b = edgeSqaredLength* (2.f *velocity.dotProduct(baseToVertex)) - 586 b = edgeSqaredLength* (2.f *velocity.dotProduct(baseToVertex)) -
587 2.0f*edgeDotVelocity*edgeDotBaseToVertex; 587 2.0f*edgeDotVelocity*edgeDotBaseToVertex;
588 c = edgeSqaredLength* (1.f -baseToVertex.getLengthSQ()) + 588 c = edgeSqaredLength* (1.f -baseToVertex.getLengthSQ()) +
589 edgeDotBaseToVertex*edgeDotBaseToVertex; 589 edgeDotBaseToVertex*edgeDotBaseToVertex;
590 590
591 // does the swept sphere collide against infinite edge? 591 // does the swept sphere collide against infinite edge?
592 if (getLowestRoot(a,b,c,t,&newT)) 592 if (getLowestRoot(a,b,c,t,&newT))
593 { 593 {
594 f32 f = (edgeDotVelocity*newT - edgeDotBaseToVertex) / edgeSqaredLength; 594 f32 f = (edgeDotVelocity*newT - edgeDotBaseToVertex) / edgeSqaredLength;
595 if (f >=0.0f && f <= 1.0f) 595 if (f >=0.0f && f <= 1.0f)
596 { 596 {
597 // intersection took place within segment 597 // intersection took place within segment
598 t = newT; 598 t = newT;
599 foundCollision = true; 599 foundCollision = true;
600 collisionPoint = triangle.pointA + (edge*f); 600 collisionPoint = triangle.pointA + (edge*f);
601 } 601 }
602 } 602 }
603 603
604 // p2 --- p3 604 // p2 --- p3
605 edge = triangle.pointC-triangle.pointB; 605 edge = triangle.pointC-triangle.pointB;
606 baseToVertex = triangle.pointB - base; 606 baseToVertex = triangle.pointB - base;
607 edgeSqaredLength = edge.getLengthSQ(); 607 edgeSqaredLength = edge.getLengthSQ();
608 edgeDotVelocity = edge.dotProduct(velocity); 608 edgeDotVelocity = edge.dotProduct(velocity);
609 edgeDotBaseToVertex = edge.dotProduct(baseToVertex); 609 edgeDotBaseToVertex = edge.dotProduct(baseToVertex);
610 610
611 // calculate parameters for equation 611 // calculate parameters for equation
612 a = edgeSqaredLength* -velocitySqaredLength + 612 a = edgeSqaredLength* -velocitySqaredLength +
613 edgeDotVelocity*edgeDotVelocity; 613 edgeDotVelocity*edgeDotVelocity;
614 b = edgeSqaredLength* (2*velocity.dotProduct(baseToVertex)) - 614 b = edgeSqaredLength* (2*velocity.dotProduct(baseToVertex)) -
615 2.0f*edgeDotVelocity*edgeDotBaseToVertex; 615 2.0f*edgeDotVelocity*edgeDotBaseToVertex;
616 c = edgeSqaredLength* (1-baseToVertex.getLengthSQ()) + 616 c = edgeSqaredLength* (1-baseToVertex.getLengthSQ()) +
617 edgeDotBaseToVertex*edgeDotBaseToVertex; 617 edgeDotBaseToVertex*edgeDotBaseToVertex;
618 618
619 // does the swept sphere collide against infinite edge? 619 // does the swept sphere collide against infinite edge?
620 if (getLowestRoot(a,b,c,t,&newT)) 620 if (getLowestRoot(a,b,c,t,&newT))
621 { 621 {
622 f32 f = (edgeDotVelocity*newT-edgeDotBaseToVertex) / 622 f32 f = (edgeDotVelocity*newT-edgeDotBaseToVertex) /
623 edgeSqaredLength; 623 edgeSqaredLength;
624 if (f >=0.0f && f <= 1.0f) 624 if (f >=0.0f && f <= 1.0f)
625 { 625 {
626 // intersection took place within segment 626 // intersection took place within segment
627 t = newT; 627 t = newT;
628 foundCollision = true; 628 foundCollision = true;
629 collisionPoint = triangle.pointB + (edge*f); 629 collisionPoint = triangle.pointB + (edge*f);
630 } 630 }
631 } 631 }
632 632
633 633
634 // p3 --- p1 634 // p3 --- p1
635 edge = triangle.pointA-triangle.pointC; 635 edge = triangle.pointA-triangle.pointC;
636 baseToVertex = triangle.pointC - base; 636 baseToVertex = triangle.pointC - base;
637 edgeSqaredLength = edge.getLengthSQ(); 637 edgeSqaredLength = edge.getLengthSQ();
638 edgeDotVelocity = edge.dotProduct(velocity); 638 edgeDotVelocity = edge.dotProduct(velocity);
639 edgeDotBaseToVertex = edge.dotProduct(baseToVertex); 639 edgeDotBaseToVertex = edge.dotProduct(baseToVertex);
640 640
641 // calculate parameters for equation 641 // calculate parameters for equation
642 a = edgeSqaredLength* -velocitySqaredLength + 642 a = edgeSqaredLength* -velocitySqaredLength +
643 edgeDotVelocity*edgeDotVelocity; 643 edgeDotVelocity*edgeDotVelocity;
644 b = edgeSqaredLength* (2*velocity.dotProduct(baseToVertex)) - 644 b = edgeSqaredLength* (2*velocity.dotProduct(baseToVertex)) -
645 2.0f*edgeDotVelocity*edgeDotBaseToVertex; 645 2.0f*edgeDotVelocity*edgeDotBaseToVertex;
646 c = edgeSqaredLength* (1-baseToVertex.getLengthSQ()) + 646 c = edgeSqaredLength* (1-baseToVertex.getLengthSQ()) +
647 edgeDotBaseToVertex*edgeDotBaseToVertex; 647 edgeDotBaseToVertex*edgeDotBaseToVertex;
648 648
649 // does the swept sphere collide against infinite edge? 649 // does the swept sphere collide against infinite edge?
650 if (getLowestRoot(a,b,c,t,&newT)) 650 if (getLowestRoot(a,b,c,t,&newT))
651 { 651 {
652 f32 f = (edgeDotVelocity*newT-edgeDotBaseToVertex) / 652 f32 f = (edgeDotVelocity*newT-edgeDotBaseToVertex) /
653 edgeSqaredLength; 653 edgeSqaredLength;
654 if (f >=0.0f && f <= 1.0f) 654 if (f >=0.0f && f <= 1.0f)
655 { 655 {
656 // intersection took place within segment 656 // intersection took place within segment
657 t = newT; 657 t = newT;
658 foundCollision = true; 658 foundCollision = true;
659 collisionPoint = triangle.pointC + (edge*f); 659 collisionPoint = triangle.pointC + (edge*f);
660 } 660 }
661 } 661 }
662 }// end no collision found 662 }// end no collision found
663 663
664 // set result: 664 // set result:
665 if (foundCollision) 665 if (foundCollision)
666 { 666 {
667 // distance to collision is t 667 // distance to collision is t
668 f32 distToCollision = t*colData->velocity.getLength(); 668 f32 distToCollision = t*colData->velocity.getLength();
669 669
670 // does this triangle qualify for closest hit? 670 // does this triangle qualify for closest hit?
671 if (!colData->foundCollision || 671 if (!colData->foundCollision ||
672 distToCollision < colData->nearestDistance) 672 distToCollision < colData->nearestDistance)
673 { 673 {
674 colData->nearestDistance = distToCollision; 674 colData->nearestDistance = distToCollision;
675 colData->intersectionPoint = collisionPoint; 675 colData->intersectionPoint = collisionPoint;
676 colData->foundCollision = true; 676 colData->foundCollision = true;
677 colData->intersectionTriangle = triangle; 677 colData->intersectionTriangle = triangle;
678 ++colData->triangleHits; 678 ++colData->triangleHits;
679 return true; 679 return true;
680 } 680 }
681 }// end found collision 681 }// end found collision
682 682
683 return false; 683 return false;
684} 684}
685 685
686 686
687//! Collides a moving ellipsoid with a 3d world with gravity and returns 687//! Collides a moving ellipsoid with a 3d world with gravity and returns
688//! the resulting new position of the ellipsoid. 688//! the resulting new position of the ellipsoid.
689core::vector3df CSceneCollisionManager::collideEllipsoidWithWorld( 689core::vector3df CSceneCollisionManager::collideEllipsoidWithWorld(
690 ITriangleSelector* selector, const core::vector3df &position, 690 ITriangleSelector* selector, const core::vector3df &position,
691 const core::vector3df& radius, const core::vector3df& velocity, 691 const core::vector3df& radius, const core::vector3df& velocity,
692 f32 slidingSpeed, 692 f32 slidingSpeed,
693 const core::vector3df& gravity, 693 const core::vector3df& gravity,
694 core::triangle3df& triout, 694 core::triangle3df& triout,
695 core::vector3df& hitPosition, 695 core::vector3df& hitPosition,
696 bool& outFalling, 696 bool& outFalling,
697 ISceneNode*& outNode) 697 ISceneNode*& outNode)
698{ 698{
699 if (!selector || radius.X == 0.0f || radius.Y == 0.0f || radius.Z == 0.0f) 699 if (!selector || radius.X == 0.0f || radius.Y == 0.0f || radius.Z == 0.0f)
700 return position; 700 return position;
701 701
702 // This code is based on the paper "Improved Collision detection and Response" 702 // This code is based on the paper "Improved Collision detection and Response"
703 // by Kasper Fauerby, but some parts are modified. 703 // by Kasper Fauerby, but some parts are modified.
704 704
705 SCollisionData colData; 705 SCollisionData colData;
706 colData.R3Position = position; 706 colData.R3Position = position;
707 colData.R3Velocity = velocity; 707 colData.R3Velocity = velocity;
708 colData.eRadius = radius; 708 colData.eRadius = radius;
709 colData.nearestDistance = FLT_MAX; 709 colData.nearestDistance = FLT_MAX;
710 colData.selector = selector; 710 colData.selector = selector;
711 colData.slidingSpeed = slidingSpeed; 711 colData.slidingSpeed = slidingSpeed;
712 colData.triangleHits = 0; 712 colData.triangleHits = 0;
713 colData.triangleIndex = -1; 713 colData.triangleIndex = -1;
714 714
715 core::vector3df eSpacePosition = colData.R3Position / colData.eRadius; 715 core::vector3df eSpacePosition = colData.R3Position / colData.eRadius;
716 core::vector3df eSpaceVelocity = colData.R3Velocity / colData.eRadius; 716 core::vector3df eSpaceVelocity = colData.R3Velocity / colData.eRadius;
717 717
718 // iterate until we have our final position 718 // iterate until we have our final position
719 719
720 core::vector3df finalPos = collideWithWorld( 720 core::vector3df finalPos = collideWithWorld(
721 0, colData, eSpacePosition, eSpaceVelocity); 721 0, colData, eSpacePosition, eSpaceVelocity);
722 722
723 outFalling = false; 723 outFalling = false;
724 724
725 // add gravity 725 // add gravity
726 726
727 if (gravity != core::vector3df(0,0,0)) 727 if (gravity != core::vector3df(0,0,0))
728 { 728 {
729 colData.R3Position = finalPos * colData.eRadius; 729 colData.R3Position = finalPos * colData.eRadius;
730 colData.R3Velocity = gravity; 730 colData.R3Velocity = gravity;
731 colData.triangleHits = 0; 731 colData.triangleHits = 0;
732 732
733 eSpaceVelocity = gravity/colData.eRadius; 733 eSpaceVelocity = gravity/colData.eRadius;
734 734
735 finalPos = collideWithWorld(0, colData, 735 finalPos = collideWithWorld(0, colData,
736 finalPos, eSpaceVelocity); 736 finalPos, eSpaceVelocity);
737 737
738 outFalling = (colData.triangleHits == 0); 738 outFalling = (colData.triangleHits == 0);
739 } 739 }
740 740
741 if (colData.triangleHits) 741 if (colData.triangleHits)
742 { 742 {
743 triout = colData.intersectionTriangle; 743 triout = colData.intersectionTriangle;
744 triout.pointA *= colData.eRadius; 744 triout.pointA *= colData.eRadius;
745 triout.pointB *= colData.eRadius; 745 triout.pointB *= colData.eRadius;
746 triout.pointC *= colData.eRadius; 746 triout.pointC *= colData.eRadius;
747 outNode = selector->getSceneNodeForTriangle(colData.triangleIndex); 747 outNode = selector->getSceneNodeForTriangle(colData.triangleIndex);
748 } 748 }
749 749
750 finalPos *= colData.eRadius; 750 finalPos *= colData.eRadius;
751 hitPosition = colData.intersectionPoint * colData.eRadius; 751 hitPosition = colData.intersectionPoint * colData.eRadius;
752 return finalPos; 752 return finalPos;
753} 753}
754 754
755 755
756core::vector3df CSceneCollisionManager::collideWithWorld(s32 recursionDepth, 756core::vector3df CSceneCollisionManager::collideWithWorld(s32 recursionDepth,
757 SCollisionData &colData, core::vector3df pos, core::vector3df vel) 757 SCollisionData &colData, core::vector3df pos, core::vector3df vel)
758{ 758{
759 f32 veryCloseDistance = colData.slidingSpeed; 759 f32 veryCloseDistance = colData.slidingSpeed;
760 760
761 if (recursionDepth > 5) 761 if (recursionDepth > 5)
762 return pos; 762 return pos;
763 763
764 colData.velocity = vel; 764 colData.velocity = vel;
765 colData.normalizedVelocity = vel; 765 colData.normalizedVelocity = vel;
766 colData.normalizedVelocity.normalize(); 766 colData.normalizedVelocity.normalize();
767 colData.basePoint = pos; 767 colData.basePoint = pos;
768 colData.foundCollision = false; 768 colData.foundCollision = false;
769 colData.nearestDistance = FLT_MAX; 769 colData.nearestDistance = FLT_MAX;
770 770
771 //------------------ collide with world 771 //------------------ collide with world
772 772
773 // get all triangles with which we might collide 773 // get all triangles with which we might collide
774 core::aabbox3d<f32> box(colData.R3Position); 774 core::aabbox3d<f32> box(colData.R3Position);
775 box.addInternalPoint(colData.R3Position + colData.R3Velocity); 775 box.addInternalPoint(colData.R3Position + colData.R3Velocity);
776 box.MinEdge -= colData.eRadius; 776 box.MinEdge -= colData.eRadius;
777 box.MaxEdge += colData.eRadius; 777 box.MaxEdge += colData.eRadius;
778 778
779 s32 totalTriangleCnt = colData.selector->getTriangleCount(); 779 s32 totalTriangleCnt = colData.selector->getTriangleCount();
780 Triangles.set_used(totalTriangleCnt); 780 Triangles.set_used(totalTriangleCnt);
781 781
782 core::matrix4 scaleMatrix; 782 core::matrix4 scaleMatrix;
783 scaleMatrix.setScale( 783 scaleMatrix.setScale(
784 core::vector3df(1.0f / colData.eRadius.X, 784 core::vector3df(1.0f / colData.eRadius.X,
785 1.0f / colData.eRadius.Y, 785 1.0f / colData.eRadius.Y,
786 1.0f / colData.eRadius.Z)); 786 1.0f / colData.eRadius.Z));
787 787
788 s32 triangleCnt = 0; 788 s32 triangleCnt = 0;
789 colData.selector->getTriangles(Triangles.pointer(), totalTriangleCnt, triangleCnt, box, &scaleMatrix); 789 colData.selector->getTriangles(Triangles.pointer(), totalTriangleCnt, triangleCnt, box, &scaleMatrix);
790 790
791 for (s32 i=0; i<triangleCnt; ++i) 791 for (s32 i=0; i<triangleCnt; ++i)
792 if(testTriangleIntersection(&colData, Triangles[i])) 792 if(testTriangleIntersection(&colData, Triangles[i]))
793 colData.triangleIndex = i; 793 colData.triangleIndex = i;
794 794
795 //---------------- end collide with world 795 //---------------- end collide with world
796 796
797 if (!colData.foundCollision) 797 if (!colData.foundCollision)
798 return pos + vel; 798 return pos + vel;
799 799
800 // original destination point 800 // original destination point
801 const core::vector3df destinationPoint = pos + vel; 801 const core::vector3df destinationPoint = pos + vel;
802 core::vector3df newBasePoint = pos; 802 core::vector3df newBasePoint = pos;
803 803
804 // only update if we are not already very close 804 // only update if we are not already very close
805 // and if so only move very close to intersection, not to the 805 // and if so only move very close to intersection, not to the
806 // exact point 806 // exact point
807 if (colData.nearestDistance >= veryCloseDistance) 807 if (colData.nearestDistance >= veryCloseDistance)
808 { 808 {
809 core::vector3df v = vel; 809 core::vector3df v = vel;
810 v.setLength( colData.nearestDistance - veryCloseDistance ); 810 v.setLength( colData.nearestDistance - veryCloseDistance );
811 newBasePoint = colData.basePoint + v; 811 newBasePoint = colData.basePoint + v;
812 812
813 v.normalize(); 813 v.normalize();
814 colData.intersectionPoint -= (v * veryCloseDistance); 814 colData.intersectionPoint -= (v * veryCloseDistance);
815 } 815 }
816 816
817 // calculate sliding plane 817 // calculate sliding plane
818 818
819 const core::vector3df slidePlaneOrigin = colData.intersectionPoint; 819 const core::vector3df slidePlaneOrigin = colData.intersectionPoint;
820 const core::vector3df slidePlaneNormal = (newBasePoint - colData.intersectionPoint).normalize(); 820 const core::vector3df slidePlaneNormal = (newBasePoint - colData.intersectionPoint).normalize();
821 core::plane3d<f32> slidingPlane(slidePlaneOrigin, slidePlaneNormal); 821 core::plane3d<f32> slidingPlane(slidePlaneOrigin, slidePlaneNormal);
822 822
823 core::vector3df newDestinationPoint = 823 core::vector3df newDestinationPoint =
824 destinationPoint - 824 destinationPoint -
825 (slidePlaneNormal * slidingPlane.getDistanceTo(destinationPoint)); 825 (slidePlaneNormal * slidingPlane.getDistanceTo(destinationPoint));
826 826
827 // generate slide vector 827 // generate slide vector
828 828
829 const core::vector3df newVelocityVector = newDestinationPoint - 829 const core::vector3df newVelocityVector = newDestinationPoint -
830 colData.intersectionPoint; 830 colData.intersectionPoint;
831 831
832 if (newVelocityVector.getLength() < veryCloseDistance) 832 if (newVelocityVector.getLength() < veryCloseDistance)
833 return newBasePoint; 833 return newBasePoint;
834 834
835 return collideWithWorld(recursionDepth+1, colData, 835 return collideWithWorld(recursionDepth+1, colData,
836 newBasePoint, newVelocityVector); 836 newBasePoint, newVelocityVector);
837} 837}
838 838
839 839
840//! Returns a 3d ray which would go through the 2d screen coodinates. 840//! Returns a 3d ray which would go through the 2d screen coodinates.
841core::line3d<f32> CSceneCollisionManager::getRayFromScreenCoordinates( 841core::line3d<f32> CSceneCollisionManager::getRayFromScreenCoordinates(
842 const core::position2d<s32> & pos, ICameraSceneNode* camera) 842 const core::position2d<s32> & pos, ICameraSceneNode* camera)
843{ 843{
844 core::line3d<f32> ln(0,0,0,0,0,0); 844 core::line3d<f32> ln(0,0,0,0,0,0);
845 845
846 if (!SceneManager) 846 if (!SceneManager)
847 return ln; 847 return ln;
848 848
849 if (!camera) 849 if (!camera)
850 camera = SceneManager->getActiveCamera(); 850 camera = SceneManager->getActiveCamera();
851 851
852 if (!camera) 852 if (!camera)
853 return ln; 853 return ln;
854 854
855 const scene::SViewFrustum* f = camera->getViewFrustum(); 855 const scene::SViewFrustum* f = camera->getViewFrustum();
856 856
857 core::vector3df farLeftUp = f->getFarLeftUp(); 857 core::vector3df farLeftUp = f->getFarLeftUp();
858 core::vector3df lefttoright = f->getFarRightUp() - farLeftUp; 858 core::vector3df lefttoright = f->getFarRightUp() - farLeftUp;
859 core::vector3df uptodown = f->getFarLeftDown() - farLeftUp; 859 core::vector3df uptodown = f->getFarLeftDown() - farLeftUp;
860 860
861 const core::rect<s32>& viewPort = Driver->getViewPort(); 861 const core::rect<s32>& viewPort = Driver->getViewPort();
862 core::dimension2d<u32> screenSize(viewPort.getWidth(), viewPort.getHeight()); 862 core::dimension2d<u32> screenSize(viewPort.getWidth(), viewPort.getHeight());
863 863
864 f32 dx = pos.X / (f32)screenSize.Width; 864 f32 dx = pos.X / (f32)screenSize.Width;
865 f32 dy = pos.Y / (f32)screenSize.Height; 865 f32 dy = pos.Y / (f32)screenSize.Height;
866 866
867 if (camera->isOrthogonal()) 867 if (camera->isOrthogonal())
868 ln.start = f->cameraPosition + (lefttoright * (dx-0.5f)) + (uptodown * (dy-0.5f)); 868 ln.start = f->cameraPosition + (lefttoright * (dx-0.5f)) + (uptodown * (dy-0.5f));
869 else 869 else
870 ln.start = f->cameraPosition; 870 ln.start = f->cameraPosition;
871 871
872 ln.end = farLeftUp + (lefttoright * dx) + (uptodown * dy); 872 ln.end = farLeftUp + (lefttoright * dx) + (uptodown * dy);
873 873
874 return ln; 874 return ln;
875} 875}
876 876
877 877
878//! Calculates 2d screen position from a 3d position. 878//! Calculates 2d screen position from a 3d position.
879core::position2d<s32> CSceneCollisionManager::getScreenCoordinatesFrom3DPosition( 879core::position2d<s32> CSceneCollisionManager::getScreenCoordinatesFrom3DPosition(
880 const core::vector3df & pos3d, ICameraSceneNode* camera, bool useViewPort) 880 const core::vector3df & pos3d, ICameraSceneNode* camera, bool useViewPort)
881{ 881{
882 if (!SceneManager || !Driver) 882 if (!SceneManager || !Driver)
883 return core::position2d<s32>(-1000,-1000); 883 return core::position2d<s32>(-1000,-1000);
884 884
885 if (!camera) 885 if (!camera)
886 camera = SceneManager->getActiveCamera(); 886 camera = SceneManager->getActiveCamera();
887 887
888 if (!camera) 888 if (!camera)
889 return core::position2d<s32>(-1000,-1000); 889 return core::position2d<s32>(-1000,-1000);
890 890
891 core::dimension2d<u32> dim; 891 core::dimension2d<u32> dim;
892 if (useViewPort) 892 if (useViewPort)
893 dim.set(Driver->getViewPort().getWidth(), Driver->getViewPort().getHeight()); 893 dim.set(Driver->getViewPort().getWidth(), Driver->getViewPort().getHeight());
894 else 894 else
895 dim=(Driver->getCurrentRenderTargetSize()); 895 dim=(Driver->getCurrentRenderTargetSize());
896 896
897 dim.Width /= 2; 897 dim.Width /= 2;
898 dim.Height /= 2; 898 dim.Height /= 2;
899 899
900 core::matrix4 trans = camera->getProjectionMatrix(); 900 core::matrix4 trans = camera->getProjectionMatrix();
901 trans *= camera->getViewMatrix(); 901 trans *= camera->getViewMatrix();
902 902
903 f32 transformedPos[4] = { pos3d.X, pos3d.Y, pos3d.Z, 1.0f }; 903 f32 transformedPos[4] = { pos3d.X, pos3d.Y, pos3d.Z, 1.0f };
904 904
905 trans.multiplyWith1x4Matrix(transformedPos); 905 trans.multiplyWith1x4Matrix(transformedPos);
906 906
907 if (transformedPos[3] < 0) 907 if (transformedPos[3] < 0)
908 return core::position2d<s32>(-10000,-10000); 908 return core::position2d<s32>(-10000,-10000);
909 909
910 const f32 zDiv = transformedPos[3] == 0.0f ? 1.0f : 910 const f32 zDiv = transformedPos[3] == 0.0f ? 1.0f :
911 core::reciprocal(transformedPos[3]); 911 core::reciprocal(transformedPos[3]);
912 912
913 return core::position2d<s32>( 913 return core::position2d<s32>(
914 dim.Width + core::round32(dim.Width * (transformedPos[0] * zDiv)), 914 dim.Width + core::round32(dim.Width * (transformedPos[0] * zDiv)),
915 dim.Height - core::round32(dim.Height * (transformedPos[1] * zDiv))); 915 dim.Height - core::round32(dim.Height * (transformedPos[1] * zDiv)));
916} 916}
917 917
918 918
919inline bool CSceneCollisionManager::getLowestRoot(f32 a, f32 b, f32 c, f32 maxR, f32* root) 919inline bool CSceneCollisionManager::getLowestRoot(f32 a, f32 b, f32 c, f32 maxR, f32* root)
920{ 920{
921 // check if solution exists 921 // check if solution exists
922 const f32 determinant = b*b - 4.0f*a*c; 922 const f32 determinant = b*b - 4.0f*a*c;
923 923
924 // if determinant is negative, no solution 924 // if determinant is negative, no solution
925 if (determinant < 0.0f || a == 0.f ) 925 if (determinant < 0.0f || a == 0.f )
926 return false; 926 return false;
927 927
928 // calculate two roots: (if det==0 then x1==x2 928 // calculate two roots: (if det==0 then x1==x2
929 // but lets disregard that slight optimization) 929 // but lets disregard that slight optimization)
930 930
931 const f32 sqrtD = sqrtf(determinant); 931 const f32 sqrtD = sqrtf(determinant);
932 const f32 invDA = core::reciprocal(2*a); 932 const f32 invDA = core::reciprocal(2*a);
933 f32 r1 = (-b - sqrtD) * invDA; 933 f32 r1 = (-b - sqrtD) * invDA;
934 f32 r2 = (-b + sqrtD) * invDA; 934 f32 r2 = (-b + sqrtD) * invDA;
935 935
936 // sort so x1 <= x2 936 // sort so x1 <= x2
937 if (r1 > r2) 937 if (r1 > r2)
938 core::swap(r1,r2); 938 core::swap(r1,r2);
939 939
940 // get lowest root 940 // get lowest root
941 if (r1 > 0 && r1 < maxR) 941 if (r1 > 0 && r1 < maxR)
942 { 942 {
943 *root = r1; 943 *root = r1;
944 return true; 944 return true;
945 } 945 }
946 946
947 // its possible that we want x2, this can happen if x1 < 0 947 // its possible that we want x2, this can happen if x1 < 0
948 if (r2 > 0 && r2 < maxR) 948 if (r2 > 0 && r2 < maxR)
949 { 949 {
950 *root = r2; 950 *root = r2;
951 return true; 951 return true;
952 } 952 }
953 953
954 return false; 954 return false;
955} 955}
956 956
957 957
958} // end namespace scene 958} // end namespace scene
959} // end namespace irr 959} // end namespace irr
960 960
generated by cgit v1.1 at 2024-07-17 23:04:04 +0000