Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/ode/ode-0.9/OPCODE/OPC_SphereCollider.cpp @ 218

Last change on this file since 218 was 216, checked in by mathiask, 18 years ago
[Physik] add ode-0.9
File size: 27.9 KB

Line
1	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
2	/*
3	* OPCODE - Optimized Collision Detection
4	* Copyright (C) 2001 Pierre Terdiman
5	* Homepage: http://www.codercorner.com/Opcode.htm
6	*/
7	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
8
9	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
10	/**
11	* Contains code for a sphere collider.
12	* \file OPC_SphereCollider.cpp
13	* \author Pierre Terdiman
14	* \date June, 2, 2001
15	*/
16	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
17
18	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
19	/**
20	* Contains a sphere-vs-tree collider.
21	* This class performs a collision test between a sphere and an AABB tree. You can use this to do a standard player vs world collision,
22	* in a Nettle/Telemachos way. It doesn't suffer from all reported bugs in those two classic codes - the "new" one by Paul Nettle is a
23	* debuggued version I think. Collision response can be driven by reported collision data - it works extremely well for me. In sake of
24	* efficiency, all meshes (that is, all AABB trees) should of course also be kept in an extra hierarchical structure (octree, whatever).
25	*
26	* \class SphereCollider
27	* \author Pierre Terdiman
28	* \version 1.3
29	* \date June, 2, 2001
30	*/
31	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
32
33	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
34	// Precompiled Header
35	#include "Stdafx.h"
36
37	using namespace Opcode;
38
39	#include "OPC_SphereAABBOverlap.h"
40	#include "OPC_SphereTriOverlap.h"
41
42	#define SET_CONTACT(prim_index, flag) \
43	/* Set contact status */ \
44	mFlags \|= flag; \
45	mTouchedPrimitives->Add(udword(prim_index));
46
47	//! Sphere-triangle overlap test
48	#define SPHERE_PRIM(prim_index, flag) \
49	/* Request vertices from the app */ \
50	VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \
51	\
52	/* Perform sphere-tri overlap test */ \
53	if(SphereTriOverlap(VP.Vertex[0], VP.Vertex[1], *VP.Vertex[2])) \
54	{ \
55	SET_CONTACT(prim_index, flag) \
56	}
57
58	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
59	/**
60	* Constructor.
61	*/
62	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
63	SphereCollider::SphereCollider()
64	{
65	mCenter.Zero();
66	mRadius2 = 0.0f;
67	}
68
69	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
70	/**
71	* Destructor.
72	*/
73	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
74	SphereCollider::~SphereCollider()
75	{
76	}
77
78	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
79	/**
80	* Generic collision query for generic OPCODE models. After the call, access the results:
81	* - with GetContactStatus()
82	* - with GetNbTouchedPrimitives()
83	* - with GetTouchedPrimitives()
84	*
85	* \param cache [in/out] a sphere cache
86	* \param sphere [in] collision sphere in local space
87	* \param model [in] Opcode model to collide with
88	* \param worlds [in] sphere's world matrix, or null
89	* \param worldm [in] model's world matrix, or null
90	* \return true if success
91	* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
92	*/
93	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
94	bool SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const Model& model, const Matrix4x4* worlds, const Matrix4x4* worldm)
95	{
96	// Checkings
97	if(!Setup(&model)) return false;
98
99	// Init collision query
100	if(InitQuery(cache, sphere, worlds, worldm)) return true;
101
102	// Special case for 1-leaf trees
103	if(mCurrentModel && mCurrentModel->HasSingleNode())
104	{
105	// Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
106	udword Nb = mIMesh->GetNbTriangles();
107	// Loop through all triangles
108	for(udword i=0;i<Nb;i++)
109	{
110	SPHERE_PRIM(i, OPC_CONTACT)
111	}
112	return true;
113	}
114
115	if(!model.HasLeafNodes())
116	{
117	if(model.IsQuantized())
118	{
119	const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
120
121	// Setup dequantization coeffs
122	mCenterCoeff = Tree->mCenterCoeff;
123	mExtentsCoeff = Tree->mExtentsCoeff;
124
125	// Perform collision query
126	if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
127	else _Collide(Tree->GetNodes());
128	}
129	else
130	{
131	const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
132
133	// Perform collision query
134	if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
135	else _Collide(Tree->GetNodes());
136	}
137	}
138	else
139	{
140	if(model.IsQuantized())
141	{
142	const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
143
144	// Setup dequantization coeffs
145	mCenterCoeff = Tree->mCenterCoeff;
146	mExtentsCoeff = Tree->mExtentsCoeff;
147
148	// Perform collision query
149	if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
150	else _Collide(Tree->GetNodes());
151	}
152	else
153	{
154	const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
155
156	// Perform collision query
157	if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
158	else _Collide(Tree->GetNodes());
159	}
160	}
161	return true;
162	}
163
164	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
165	/**
166	* Initializes a collision query :
167	* - reset stats & contact status
168	* - setup matrices
169	* - check temporal coherence
170	*
171	* \param cache [in/out] a sphere cache
172	* \param sphere [in] sphere in local space
173	* \param worlds [in] sphere's world matrix, or null
174	* \param worldm [in] model's world matrix, or null
175	* \return TRUE if we can return immediately
176	* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
177	*/
178	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
179	BOOL SphereCollider::InitQuery(SphereCache& cache, const Sphere& sphere, const Matrix4x4* worlds, const Matrix4x4* worldm)
180	{
181	// 1) Call the base method
182	VolumeCollider::InitQuery();
183
184	// 2) Compute sphere in model space:
185	// - Precompute R^2
186	mRadius2 = sphere.mRadius * sphere.mRadius;
187	// - Compute center position
188	mCenter = sphere.mCenter;
189	// -> to world space
190	if(worlds) mCenter = worlds;
191	// -> to model space
192	if(worldm)
193	{
194	// Invert model matrix
195	Matrix4x4 InvWorldM;
196	InvertPRMatrix(InvWorldM, *worldm);
197
198	mCenter *= InvWorldM;
199	}
200
201	// 3) Setup destination pointer
202	mTouchedPrimitives = &cache.TouchedPrimitives;
203
204	// 4) Special case: 1-triangle meshes [Opcode 1.3]
205	if(mCurrentModel && mCurrentModel->HasSingleNode())
206	{
207	if(!SkipPrimitiveTests())
208	{
209	// We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
210	mTouchedPrimitives->Reset();
211
212	// Perform overlap test between the unique triangle and the sphere (and set contact status if needed)
213	SPHERE_PRIM(udword(0), OPC_CONTACT)
214
215	// Return immediately regardless of status
216	return TRUE;
217	}
218	}
219
220	// 5) Check temporal coherence :
221	if(TemporalCoherenceEnabled())
222	{
223	// Here we use temporal coherence
224	// => check results from previous frame before performing the collision query
225	if(FirstContactEnabled())
226	{
227	// We're only interested in the first contact found => test the unique previously touched face
228	if(mTouchedPrimitives->GetNbEntries())
229	{
230	// Get index of previously touched face = the first entry in the array
231	udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0);
232
233	// Then reset the array:
234	// - if the overlap test below is successful, the index we'll get added back anyway
235	// - if it isn't, then the array should be reset anyway for the normal query
236	mTouchedPrimitives->Reset();
237
238	// Perform overlap test between the cached triangle and the sphere (and set contact status if needed)
239	SPHERE_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT)
240
241	// Return immediately if possible
242	if(GetContactStatus()) return TRUE;
243	}
244	// else no face has been touched during previous query
245	// => we'll have to perform a normal query
246	}
247	else
248	{
249	// We're interested in all contacts =>test the new real sphere N(ew) against the previous fat sphere P(revious):
250	float r = sqrtf(cache.FatRadius2) - sphere.mRadius;
251	if(IsCacheValid(cache) && cache.Center.SquareDistance(mCenter) < r*r)
252	{
253	// - if N is included in P, return previous list
254	// => we simply leave the list (mTouchedFaces) unchanged
255
256	// Set contact status if needed
257	if(mTouchedPrimitives->GetNbEntries()) mFlags \|= OPC_TEMPORAL_CONTACT;
258
259	// In any case we don't need to do a query
260	return TRUE;
261	}
262	else
263	{
264	// - else do the query using a fat N
265
266	// Reset cache since we'll about to perform a real query
267	mTouchedPrimitives->Reset();
268
269	// Make a fat sphere so that coherence will work for subsequent frames
270	mRadius2 *= cache.FatCoeff;
271	// mRadius2 = (sphere.mRadius * cache.FatCoeff)(sphere.mRadius cache.FatCoeff);
272
273	// Update cache with query data (signature for cached faces)
274	cache.Center = mCenter;
275	cache.FatRadius2 = mRadius2;
276	}
277	}
278	}
279	else
280	{
281	// Here we don't use temporal coherence => do a normal query
282	mTouchedPrimitives->Reset();
283	}
284
285	return FALSE;
286	}
287
288	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
289	/**
290	* Collision query for vanilla AABB trees.
291	* \param cache [in/out] a sphere cache
292	* \param sphere [in] collision sphere in world space
293	* \param tree [in] AABB tree
294	* \return true if success
295	*/
296	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
297	bool SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const AABBTree* tree)
298	{
299	// This is typically called for a scene tree, full of -AABBs-, not full of triangles.
300	// So we don't really have "primitives" to deal with. Hence it doesn't work with
301	// "FirstContact" + "TemporalCoherence".
302	ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) );
303
304	// Checkings
305	if(!tree) return false;
306
307	// Init collision query
308	if(InitQuery(cache, sphere)) return true;
309
310	// Perform collision query
311	_Collide(tree);
312
313	return true;
314	}
315
316	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
317	/**
318	* Checks the sphere completely contains the box. In which case we can end the query sooner.
319	* \param bc [in] box center
320	* \param be [in] box extents
321	* \return true if the sphere contains the whole box
322	*/
323	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
324	inline_ BOOL SphereCollider::SphereContainsBox(const Point& bc, const Point& be)
325	{
326	// I assume if all 8 box vertices are inside the sphere, so does the whole box.
327	// Sounds ok but maybe there's a better way?
328	Point p;
329	p.x=bc.x+be.x; p.y=bc.y+be.y; p.z=bc.z+be.z; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
330	p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
331	p.x=bc.x+be.x; p.y=bc.y-be.y; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
332	p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
333	p.x=bc.x+be.x; p.y=bc.y+be.y; p.z=bc.z-be.z; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
334	p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
335	p.x=bc.x+be.x; p.y=bc.y-be.y; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
336	p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
337
338	return TRUE;
339	}
340
341	#define TEST_BOX_IN_SPHERE(center, extents) \
342	if(SphereContainsBox(center, extents)) \
343	{ \
344	/* Set contact status */ \
345	mFlags \|= OPC_CONTACT; \
346	_Dump(node); \
347	return; \
348	}
349
350	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
351	/**
352	* Recursive collision query for normal AABB trees.
353	* \param node [in] current collision node
354	*/
355	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
356	void SphereCollider::_Collide(const AABBCollisionNode* node)
357	{
358	// Perform Sphere-AABB overlap test
359	if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
360
361	TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
362
363	if(node->IsLeaf())
364	{
365	SPHERE_PRIM(node->GetPrimitive(), OPC_CONTACT)
366	}
367	else
368	{
369	_Collide(node->GetPos());
370
371	if(ContactFound()) return;
372
373	_Collide(node->GetNeg());
374	}
375	}
376
377	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
378	/**
379	* Recursive collision query for normal AABB trees, without primitive tests.
380	* \param node [in] current collision node
381	*/
382	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
383	void SphereCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node)
384	{
385	// Perform Sphere-AABB overlap test
386	if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
387
388	TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
389
390	if(node->IsLeaf())
391	{
392	SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
393	}
394	else
395	{
396	_CollideNoPrimitiveTest(node->GetPos());
397
398	if(ContactFound()) return;
399
400	_CollideNoPrimitiveTest(node->GetNeg());
401	}
402	}
403
404	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
405	/**
406	* Recursive collision query for quantized AABB trees.
407	* \param node [in] current collision node
408	*/
409	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
410	void SphereCollider::_Collide(const AABBQuantizedNode* node)
411	{
412	// Dequantize box
413	const QuantizedAABB& Box = node->mAABB;
414	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
415	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
416
417	// Perform Sphere-AABB overlap test
418	if(!SphereAABBOverlap(Center, Extents)) return;
419
420	TEST_BOX_IN_SPHERE(Center, Extents)
421
422	if(node->IsLeaf())
423	{
424	SPHERE_PRIM(node->GetPrimitive(), OPC_CONTACT)
425	}
426	else
427	{
428	_Collide(node->GetPos());
429
430	if(ContactFound()) return;
431
432	_Collide(node->GetNeg());
433	}
434	}
435
436	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
437	/**
438	* Recursive collision query for quantized AABB trees, without primitive tests.
439	* \param node [in] current collision node
440	*/
441	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
442	void SphereCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node)
443	{
444	// Dequantize box
445	const QuantizedAABB& Box = node->mAABB;
446	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
447	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
448
449	// Perform Sphere-AABB overlap test
450	if(!SphereAABBOverlap(Center, Extents)) return;
451
452	TEST_BOX_IN_SPHERE(Center, Extents)
453
454	if(node->IsLeaf())
455	{
456	SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
457	}
458	else
459	{
460	_CollideNoPrimitiveTest(node->GetPos());
461
462	if(ContactFound()) return;
463
464	_CollideNoPrimitiveTest(node->GetNeg());
465	}
466	}
467
468	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
469	/**
470	* Recursive collision query for no-leaf AABB trees.
471	* \param node [in] current collision node
472	*/
473	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
474	void SphereCollider::_Collide(const AABBNoLeafNode* node)
475	{
476	// Perform Sphere-AABB overlap test
477	if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
478
479	TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
480
481	if(node->HasPosLeaf()) { SPHERE_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
482	else _Collide(node->GetPos());
483
484	if(ContactFound()) return;
485
486	if(node->HasNegLeaf()) { SPHERE_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
487	else _Collide(node->GetNeg());
488	}
489
490	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
491	/**
492	* Recursive collision query for no-leaf AABB trees, without primitive tests.
493	* \param node [in] current collision node
494	*/
495	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
496	void SphereCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node)
497	{
498	// Perform Sphere-AABB overlap test
499	if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
500
501	TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
502
503	if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
504	else _CollideNoPrimitiveTest(node->GetPos());
505
506	if(ContactFound()) return;
507
508	if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
509	else _CollideNoPrimitiveTest(node->GetNeg());
510	}
511
512	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
513	/**
514	* Recursive collision query for quantized no-leaf AABB trees.
515	* \param node [in] current collision node
516	*/
517	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
518	void SphereCollider::_Collide(const AABBQuantizedNoLeafNode* node)
519	{
520	// Dequantize box
521	const QuantizedAABB& Box = node->mAABB;
522	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
523	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
524
525	// Perform Sphere-AABB overlap test
526	if(!SphereAABBOverlap(Center, Extents)) return;
527
528	TEST_BOX_IN_SPHERE(Center, Extents)
529
530	if(node->HasPosLeaf()) { SPHERE_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
531	else _Collide(node->GetPos());
532
533	if(ContactFound()) return;
534
535	if(node->HasNegLeaf()) { SPHERE_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
536	else _Collide(node->GetNeg());
537	}
538
539	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
540	/**
541	* Recursive collision query for quantized no-leaf AABB trees, without primitive tests.
542	* \param node [in] current collision node
543	*/
544	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
545	void SphereCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node)
546	{
547	// Dequantize box
548	const QuantizedAABB& Box = node->mAABB;
549	const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
550	const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
551
552	// Perform Sphere-AABB overlap test
553	if(!SphereAABBOverlap(Center, Extents)) return;
554
555	TEST_BOX_IN_SPHERE(Center, Extents)
556
557	if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
558	else _CollideNoPrimitiveTest(node->GetPos());
559
560	if(ContactFound()) return;
561
562	if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
563	else _CollideNoPrimitiveTest(node->GetNeg());
564	}
565
566	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
567	/**
568	* Recursive collision query for vanilla AABB trees.
569	* \param node [in] current collision node
570	*/
571	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
572	void SphereCollider::_Collide(const AABBTreeNode* node)
573	{
574	// Perform Sphere-AABB overlap test
575	Point Center, Extents;
576	node->GetAABB()->GetCenter(Center);
577	node->GetAABB()->GetExtents(Extents);
578	if(!SphereAABBOverlap(Center, Extents)) return;
579
580	if(node->IsLeaf() \|\| SphereContainsBox(Center, Extents))
581	{
582	mFlags \|= OPC_CONTACT;
583	mTouchedPrimitives->Add(node->GetPrimitives(), node->GetNbPrimitives());
584	}
585	else
586	{
587	_Collide(node->GetPos());
588	_Collide(node->GetNeg());
589	}
590	}
591
592
593
594
595
596
597
598	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
599	/**
600	* Constructor.
601	*/
602	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
603	HybridSphereCollider::HybridSphereCollider()
604	{
605	}
606
607	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
608	/**
609	* Destructor.
610	*/
611	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
612	HybridSphereCollider::~HybridSphereCollider()
613	{
614	}
615
616	bool HybridSphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const HybridModel& model, const Matrix4x4* worlds, const Matrix4x4* worldm)
617	{
618	// We don't want primitive tests here!
619	mFlags \|= OPC_NO_PRIMITIVE_TESTS;
620
621	// Checkings
622	if(!Setup(&model)) return false;
623
624	// Init collision query
625	if(InitQuery(cache, sphere, worlds, worldm)) return true;
626
627	// Special case for 1-leaf trees
628	if(mCurrentModel && mCurrentModel->HasSingleNode())
629	{
630	// Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
631	udword Nb = mIMesh->GetNbTriangles();
632
633	// Loop through all triangles
634	for(udword i=0;i<Nb;i++)
635	{
636	SPHERE_PRIM(i, OPC_CONTACT)
637	}
638	return true;
639	}
640
641	// Override destination array since we're only going to get leaf boxes here
642	mTouchedBoxes.Reset();
643	mTouchedPrimitives = &mTouchedBoxes;
644
645	// Now, do the actual query against leaf boxes
646	if(!model.HasLeafNodes())
647	{
648	if(model.IsQuantized())
649	{
650	const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
651
652	// Setup dequantization coeffs
653	mCenterCoeff = Tree->mCenterCoeff;
654	mExtentsCoeff = Tree->mExtentsCoeff;
655
656	// Perform collision query - we don't want primitive tests here!
657	_CollideNoPrimitiveTest(Tree->GetNodes());
658	}
659	else
660	{
661	const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
662
663	// Perform collision query - we don't want primitive tests here!
664	_CollideNoPrimitiveTest(Tree->GetNodes());
665	}
666	}
667	else
668	{
669	if(model.IsQuantized())
670	{
671	const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
672
673	// Setup dequantization coeffs
674	mCenterCoeff = Tree->mCenterCoeff;
675	mExtentsCoeff = Tree->mExtentsCoeff;
676
677	// Perform collision query - we don't want primitive tests here!
678	_CollideNoPrimitiveTest(Tree->GetNodes());
679	}
680	else
681	{
682	const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
683
684	// Perform collision query - we don't want primitive tests here!
685	_CollideNoPrimitiveTest(Tree->GetNodes());
686	}
687	}
688
689	// We only have a list of boxes so far
690	if(GetContactStatus())
691	{
692	// Reset contact status, since it currently only reflects collisions with leaf boxes
693	Collider::InitQuery();
694
695	// Change dest container so that we can use built-in overlap tests and get collided primitives
696	cache.TouchedPrimitives.Reset();
697	mTouchedPrimitives = &cache.TouchedPrimitives;
698
699	// Read touched leaf boxes
700	udword Nb = mTouchedBoxes.GetNbEntries();
701	const udword* Touched = mTouchedBoxes.GetEntries();
702
703	const LeafTriangles* LT = model.GetLeafTriangles();
704	const udword* Indices = model.GetIndices();
705
706	// Loop through touched leaves
707	while(Nb--)
708	{
709	const LeafTriangles& CurrentLeaf = LT[*Touched++];
710
711	// Each leaf box has a set of triangles
712	udword NbTris = CurrentLeaf.GetNbTriangles();
713	if(Indices)
714	{
715	const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
716
717	// Loop through triangles and test each of them
718	while(NbTris--)
719	{
720	udword TriangleIndex = *T++;
721	SPHERE_PRIM(TriangleIndex, OPC_CONTACT)
722	}
723	}
724	else
725	{
726	udword BaseIndex = CurrentLeaf.GetTriangleIndex();
727
728	// Loop through triangles and test each of them
729	while(NbTris--)
730	{
731	udword TriangleIndex = BaseIndex++;
732	SPHERE_PRIM(TriangleIndex, OPC_CONTACT)
733	}
734	}
735	}
736	}
737
738	return true;
739	}

Note: See TracBrowser for help on using the repository browser.

Download in other formats: