Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/ode/ode-0.9/ode/src/collision_trimesh_sphere.cpp @ 216

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

Line
1	/*************************************************************************
2	* *
3	* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4	* All rights reserved. Email: russ@q12.org Web: www.q12.org *
5	* *
6	* This library is free software; you can redistribute it and/or *
7	* modify it under the terms of EITHER: *
8	* (1) The GNU Lesser General Public License as published by the Free *
9	* Software Foundation; either version 2.1 of the License, or (at *
10	* your option) any later version. The text of the GNU Lesser *
11	* General Public License is included with this library in the *
12	* file LICENSE.TXT. *
13	* (2) The BSD-style license that is included with this library in *
14	* the file LICENSE-BSD.TXT. *
15	* *
16	* This library is distributed in the hope that it will be useful, *
17	* but WITHOUT ANY WARRANTY; without even the implied warranty of *
18	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19	* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20	* *
21	*************************************************************************/
22
23	// TriMesh code by Erwin de Vries.
24
25	#include <ode/collision.h>
26	#include <ode/matrix.h>
27	#include <ode/rotation.h>
28	#include <ode/odemath.h>
29	#include "collision_util.h"
30
31	#if dTRIMESH_ENABLED
32
33	#define TRIMESH_INTERNAL
34	#include "collision_trimesh_internal.h"
35
36	#if dTRIMESH_OPCODE
37	#define MERGECONTACTS
38
39	// Ripped from Opcode 1.1.
40	static bool GetContactData(const dVector3& Center, dReal Radius, const dVector3 Origin, const dVector3 Edge0, const dVector3 Edge1, dReal& Dist, dReal& u, dReal& v){
41
42	// now onto the bulk of the collision...
43
44	dVector3 Diff;
45	Diff[0] = Origin[0] - Center[0];
46	Diff[1] = Origin[1] - Center[1];
47	Diff[2] = Origin[2] - Center[2];
48	Diff[3] = Origin[3] - Center[3];
49
50	dReal A00 = dDOT(Edge0, Edge0);
51	dReal A01 = dDOT(Edge0, Edge1);
52	dReal A11 = dDOT(Edge1, Edge1);
53
54	dReal B0 = dDOT(Diff, Edge0);
55	dReal B1 = dDOT(Diff, Edge1);
56
57	dReal C = dDOT(Diff, Diff);
58
59	dReal Det = dFabs(A00 * A11 - A01 * A01);
60	u = A01 * B1 - A11 * B0;
61	v = A01 * B0 - A00 * B1;
62
63	dReal DistSq;
64
65	if (u + v <= Det){
66	if(u < REAL(0.0)){
67	if(v < REAL(0.0)){ // region 4
68	if(B0 < REAL(0.0)){
69	v = REAL(0.0);
70	if (-B0 >= A00){
71	u = REAL(1.0);
72	DistSq = A00 + REAL(2.0) * B0 + C;
73	}
74	else{
75	u = -B0 / A00;
76	DistSq = B0 * u + C;
77	}
78	}
79	else{
80	u = REAL(0.0);
81	if(B1 >= REAL(0.0)){
82	v = REAL(0.0);
83	DistSq = C;
84	}
85	else if(-B1 >= A11){
86	v = REAL(1.0);
87	DistSq = A11 + REAL(2.0) * B1 + C;
88	}
89	else{
90	v = -B1 / A11;
91	DistSq = B1 * v + C;
92	}
93	}
94	}
95	else{ // region 3
96	u = REAL(0.0);
97	if(B1 >= REAL(0.0)){
98	v = REAL(0.0);
99	DistSq = C;
100	}
101	else if(-B1 >= A11){
102	v = REAL(1.0);
103	DistSq = A11 + REAL(2.0) * B1 + C;
104	}
105	else{
106	v = -B1 / A11;
107	DistSq = B1 * v + C;
108	}
109	}
110	}
111	else if(v < REAL(0.0)){ // region 5
112	v = REAL(0.0);
113	if (B0 >= REAL(0.0)){
114	u = REAL(0.0);
115	DistSq = C;
116	}
117	else if (-B0 >= A00){
118	u = REAL(1.0);
119	DistSq = A00 + REAL(2.0) * B0 + C;
120	}
121	else{
122	u = -B0 / A00;
123	DistSq = B0 * u + C;
124	}
125	}
126	else{ // region 0
127	// minimum at interior point
128	if (Det == REAL(0.0)){
129	u = REAL(0.0);
130	v = REAL(0.0);
131	DistSq = FLT_MAX;
132	}
133	else{
134	dReal InvDet = REAL(1.0) / Det;
135	u *= InvDet;
136	v *= InvDet;
137	DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
138	}
139	}
140	}
141	else{
142	dReal Tmp0, Tmp1, Numer, Denom;
143
144	if(u < REAL(0.0)){ // region 2
145	Tmp0 = A01 + B0;
146	Tmp1 = A11 + B1;
147	if (Tmp1 > Tmp0){
148	Numer = Tmp1 - Tmp0;
149	Denom = A00 - REAL(2.0) * A01 + A11;
150	if (Numer >= Denom){
151	u = REAL(1.0);
152	v = REAL(0.0);
153	DistSq = A00 + REAL(2.0) * B0 + C;
154	}
155	else{
156	u = Numer / Denom;
157	v = REAL(1.0) - u;
158	DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
159	}
160	}
161	else{
162	u = REAL(0.0);
163	if(Tmp1 <= REAL(0.0)){
164	v = REAL(1.0);
165	DistSq = A11 + REAL(2.0) * B1 + C;
166	}
167	else if(B1 >= REAL(0.0)){
168	v = REAL(0.0);
169	DistSq = C;
170	}
171	else{
172	v = -B1 / A11;
173	DistSq = B1 * v + C;
174	}
175	}
176	}
177	else if(v < REAL(0.0)){ // region 6
178	Tmp0 = A01 + B1;
179	Tmp1 = A00 + B0;
180	if (Tmp1 > Tmp0){
181	Numer = Tmp1 - Tmp0;
182	Denom = A00 - REAL(2.0) * A01 + A11;
183	if (Numer >= Denom){
184	v = REAL(1.0);
185	u = REAL(0.0);
186	DistSq = A11 + REAL(2.0) * B1 + C;
187	}
188	else{
189	v = Numer / Denom;
190	u = REAL(1.0) - v;
191	DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
192	}
193	}
194	else{
195	v = REAL(0.0);
196	if (Tmp1 <= REAL(0.0)){
197	u = REAL(1.0);
198	DistSq = A00 + REAL(2.0) * B0 + C;
199	}
200	else if(B0 >= REAL(0.0)){
201	u = REAL(0.0);
202	DistSq = C;
203	}
204	else{
205	u = -B0 / A00;
206	DistSq = B0 * u + C;
207	}
208	}
209	}
210	else{ // region 1
211	Numer = A11 + B1 - A01 - B0;
212	if (Numer <= REAL(0.0)){
213	u = REAL(0.0);
214	v = REAL(1.0);
215	DistSq = A11 + REAL(2.0) * B1 + C;
216	}
217	else{
218	Denom = A00 - REAL(2.0) * A01 + A11;
219	if (Numer >= Denom){
220	u = REAL(1.0);
221	v = REAL(0.0);
222	DistSq = A00 + REAL(2.0) * B0 + C;
223	}
224	else{
225	u = Numer / Denom;
226	v = REAL(1.0) - u;
227	DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
228	}
229	}
230	}
231	}
232
233	Dist = dSqrt(dFabs(DistSq));
234
235	if (Dist <= Radius){
236	Dist = Radius - Dist;
237	return true;
238	}
239	else return false;
240	}
241
242	int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
243	dIASSERT (Stride >= (int)sizeof(dContactGeom));
244	dIASSERT (g1->type == dTriMeshClass);
245	dIASSERT (SphereGeom->type == dSphereClass);
246	dIASSERT ((Flags & NUMC_MASK) >= 1);
247
248	dxTriMesh* TriMesh = (dxTriMesh*)g1;
249
250	// Init
251	const dVector3& TLPosition = (const dVector3)dGeomGetPosition(TriMesh);
252	const dMatrix3& TLRotation = (const dMatrix3)dGeomGetRotation(TriMesh);
253
254	SphereCollider& Collider = TriMesh->_SphereCollider;
255
256	const dVector3& Position = (const dVector3)dGeomGetPosition(SphereGeom);
257	dReal Radius = dGeomSphereGetRadius(SphereGeom);
258
259	// Sphere
260	Sphere Sphere;
261	Sphere.mCenter.x = Position[0];
262	Sphere.mCenter.y = Position[1];
263	Sphere.mCenter.z = Position[2];
264	Sphere.mRadius = Radius;
265
266	Matrix4x4 amatrix;
267
268	// TC results
269	if (TriMesh->doSphereTC) {
270	dxTriMesh::SphereTC* sphereTC = 0;
271	for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
272	if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
273	sphereTC = &TriMesh->SphereTCCache[i];
274	break;
275	}
276	}
277
278	if (!sphereTC){
279	TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());
280
281	sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
282	sphereTC->Geom = SphereGeom;
283	}
284
285	// Intersect
286	Collider.SetTemporalCoherence(true);
287	Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null,
288	&MakeMatrix(TLPosition, TLRotation, amatrix));
289	}
290	else {
291	Collider.SetTemporalCoherence(false);
292	Collider.Collide(dxTriMesh::defaultSphereCache, Sphere, TriMesh->Data->BVTree, null,
293	&MakeMatrix(TLPosition, TLRotation, amatrix));
294	}
295
296	if (! Collider.GetContactStatus()) {
297	// no collision occurred
298	return 0;
299	}
300
301	// get results
302	int TriCount = Collider.GetNbTouchedPrimitives();
303	const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
304
305	if (TriCount != 0){
306	if (TriMesh->ArrayCallback != null){
307	TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
308	}
309
310	int OutTriCount = 0;
311	for (int i = 0; i < TriCount; i++){
312	if (OutTriCount == (Flags & NUMC_MASK)){
313	break;
314	}
315
316	const int TriIndex = Triangles[i];
317
318	dVector3 dv[3];
319	if (!Callback(TriMesh, SphereGeom, TriIndex))
320	continue;
321	FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
322
323	dVector3& v0 = dv[0];
324	dVector3& v1 = dv[1];
325	dVector3& v2 = dv[2];
326
327	dVector3 vu;
328	vu[0] = v1[0] - v0[0];
329	vu[1] = v1[1] - v0[1];
330	vu[2] = v1[2] - v0[2];
331	vu[3] = REAL(0.0);
332
333	dVector3 vv;
334	vv[0] = v2[0] - v0[0];
335	vv[1] = v2[1] - v0[1];
336	vv[2] = v2[2] - v0[2];
337	vv[3] = REAL(0.0);
338
339	// Get plane coefficients
340	dVector4 Plane;
341	dCROSS(Plane, =, vu, vv);
342
343	dReal Area = dSqrt(dDOT(Plane, Plane)); // We can use this later
344	Plane[0] /= Area;
345	Plane[1] /= Area;
346	Plane[2] /= Area;
347
348	Plane[3] = dDOT(Plane, v0);
349
350	/* If the center of the sphere is within the positive halfspace of the
351	* triangle's plane, allow a contact to be generated.
352	* If the center of the sphere made it into the positive halfspace of a
353	* back-facing triangle, then the physics update and/or velocity needs
354	* to be adjusted (penetration has occured anyway).
355	*/
356
357	dReal side = dDOT(Plane,Position) - Plane[3];
358
359	if(side < REAL(0.0)) {
360	continue;
361	}
362
363	dReal Depth;
364	dReal u, v;
365	if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
366	continue; // Sphere doesn't hit triangle
367	}
368
369	if (Depth < REAL(0.0)){
370	Depth = REAL(0.0);
371	}
372
373	dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
374
375	dReal w = REAL(1.0) - u - v;
376	Contact->pos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v);
377	Contact->pos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v);
378	Contact->pos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v);
379	Contact->pos[3] = REAL(0.0);
380
381	// Using normal as plane (reversed)
382	Contact->normal[0] = -Plane[0];
383	Contact->normal[1] = -Plane[1];
384	Contact->normal[2] = -Plane[2];
385	Contact->normal[3] = REAL(0.0);
386
387	// Depth returned from GetContactData is depth along
388	// contact point - sphere center direction
389	// we'll project it to contact normal
390	dVector3 dir;
391	dir[0] = Position[0]-Contact->pos[0];
392	dir[1] = Position[1]-Contact->pos[1];
393	dir[2] = Position[2]-Contact->pos[2];
394	dReal dirProj = dDOT(dir, Plane) / dSqrt(dDOT(dir, dir));
395	Contact->depth = Depth * dirProj;
396	//Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance
397	Contact->side1 = TriIndex;
398
399	//Contact->g1 = TriMesh;
400	//Contact->g2 = SphereGeom;
401
402	OutTriCount++;
403	}
404	#ifdef MERGECONTACTS // Merge all contacts into 1
405	if (OutTriCount != 0){
406	dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
407
408	if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){
409	Contact->normal[0] *= Contact->depth;
410	Contact->normal[1] *= Contact->depth;
411	Contact->normal[2] *= Contact->depth;
412	Contact->normal[3] *= Contact->depth;
413
414	for (int i = 1; i < OutTriCount; i++){
415	dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
416
417	Contact->pos[0] += TempContact->pos[0];
418	Contact->pos[1] += TempContact->pos[1];
419	Contact->pos[2] += TempContact->pos[2];
420	Contact->pos[3] += TempContact->pos[3];
421
422	Contact->normal[0] += TempContact->normal[0] * TempContact->depth;
423	Contact->normal[1] += TempContact->normal[1] * TempContact->depth;
424	Contact->normal[2] += TempContact->normal[2] * TempContact->depth;
425	Contact->normal[3] += TempContact->normal[3] * TempContact->depth;
426	}
427
428	Contact->pos[0] /= OutTriCount;
429	Contact->pos[1] /= OutTriCount;
430	Contact->pos[2] /= OutTriCount;
431	Contact->pos[3] /= OutTriCount;
432
433	// Remember to divide in square space.
434	Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal) / OutTriCount);
435
436	dNormalize3(Contact->normal);
437	}
438
439	Contact->g1 = TriMesh;
440	Contact->g2 = SphereGeom;
441
442	// TODO:
443	// Side1 now contains index of triangle that gave first hit
444	// Probably we should find index of triangle with deepest penetration
445
446	return 1;
447	}
448	else return 0;
449	#elif defined MERGECONTACTNORMALS // Merge all normals, and distribute between all contacts
450	if (OutTriCount != 0){
451	if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){
452	dVector3& Normal = SAFECONTACT(Flags, Contacts, 0, Stride)->normal;
453	Normal[0] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
454	Normal[1] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
455	Normal[2] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
456	Normal[3] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
457
458	for (int i = 1; i < OutTriCount; i++){
459	dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
460
461	Normal[0] += Contact->normal[0] * Contact->depth;
462	Normal[1] += Contact->normal[1] * Contact->depth;
463	Normal[2] += Contact->normal[2] * Contact->depth;
464	Normal[3] += Contact->normal[3] * Contact->depth;
465	}
466	dNormalize3(Normal);
467
468	for (int i = 1; i < OutTriCount; i++){
469	dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
470
471	Contact->normal[0] = Normal[0];
472	Contact->normal[1] = Normal[1];
473	Contact->normal[2] = Normal[2];
474	Contact->normal[3] = Normal[3];
475
476	Contact->g1 = TriMesh;
477	Contact->g2 = SphereGeom;
478	}
479	}
480	else{
481	SAFECONTACT(Flags, Contacts, 0, Stride)->g1 = TriMesh;
482	SAFECONTACT(Flags, Contacts, 0, Stride)->g2 = SphereGeom;
483	}
484
485	return OutTriCount;
486	}
487	else return 0;
488	#else //MERGECONTACTNORMALS // Just gather penetration depths and return
489	for (int i = 0; i < OutTriCount; i++){
490	dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
491
492	//Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal));
493
494	/*Contact->normal[0] /= Contact->depth;
495	Contact->normal[1] /= Contact->depth;
496	Contact->normal[2] /= Contact->depth;
497	Contact->normal[3] /= Contact->depth;*/
498
499	Contact->g1 = TriMesh;
500	Contact->g2 = SphereGeom;
501	}
502
503	return OutTriCount;
504	#endif // MERGECONTACTS
505	}
506	else return 0;
507	}
508	#endif // dTRIMESH_OPCODE
509
510	#if dTRIMESH_GIMPACT
511	int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride)
512	{
513	dIASSERT (Stride >= (int)sizeof(dContactGeom));
514	dIASSERT (g1->type == dTriMeshClass);
515	dIASSERT (SphereGeom->type == dSphereClass);
516	dIASSERT ((Flags & NUMC_MASK) >= 1);
517
518	dxTriMesh* TriMesh = (dxTriMesh*)g1;
519	dVector3& Position = (dVector3)dGeomGetPosition(SphereGeom);
520	dReal Radius = dGeomSphereGetRadius(SphereGeom);
521	//Create contact list
522	GDYNAMIC_ARRAY trimeshcontacts;
523	GIM_CREATE_CONTACT_LIST(trimeshcontacts);
524
525	//Collide trimeshes
526	gim_trimesh_sphere_collision(&TriMesh->m_collision_trimesh,Position,Radius,&trimeshcontacts);
527
528	if(trimeshcontacts.m_size == 0)
529	{
530	GIM_DYNARRAY_DESTROY(trimeshcontacts);
531	return 0;
532	}
533
534	GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
535
536	unsigned contactcount = trimeshcontacts.m_size;
537	unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK);
538	if (contactcount > maxcontacts)
539	{
540	contactcount = maxcontacts;
541	}
542
543	dContactGeom* pcontact;
544	unsigned i;
545
546	for (i=0;i<contactcount;i++)
547	{
548	pcontact = SAFECONTACT(Flags, Contacts, i, Stride);
549
550	pcontact->pos[0] = ptrimeshcontacts->m_point[0];
551	pcontact->pos[1] = ptrimeshcontacts->m_point[1];
552	pcontact->pos[2] = ptrimeshcontacts->m_point[2];
553	pcontact->pos[3] = REAL(1.0);
554
555	pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
556	pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
557	pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
558	pcontact->normal[3] = 0;
559
560	pcontact->depth = ptrimeshcontacts->m_depth;
561	pcontact->g1 = g1;
562	pcontact->g2 = SphereGeom;
563
564	ptrimeshcontacts++;
565	}
566
567	GIM_DYNARRAY_DESTROY(trimeshcontacts);
568
569	return (int)contactcount;
570	}
571	#endif // dTRIMESH_GIMPACT
572
573	#endif // dTRIMESH_ENABLED

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

Download in other formats: