FreeWRL / FreeX3D 4.3.0
Collision.c
1/*
2
3
4Render the children of nodes.
5
6*/
7
8/****************************************************************************
9 This file is part of the FreeWRL/FreeX3D Distribution.
10
11 Copyright 2009 CRC Canada. (http://www.crc.gc.ca)
12
13 FreeWRL/FreeX3D is free software: you can redistribute it and/or modify
14 it under the terms of the GNU Lesser Public License as published by
15 the Free Software Foundation, either version 3 of the License, or
16 (at your option) any later version.
17
18 FreeWRL/FreeX3D is distributed in the hope that it will be useful,
19 but WITHOUT ANY WARRANTY; without even the implied warranty of
20 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21 GNU General Public License for more details.
22
23 You should have received a copy of the GNU General Public License
24 along with FreeWRL/FreeX3D. If not, see <http://www.gnu.org/licenses/>.
25****************************************************************************/
26
27
28#include <config.h>
29#include <system.h>
30#include <display.h>
31#include <internal.h>
32
33#include <libFreeWRL.h>
34
35#include "Viewer.h"
36#include "../x3d_parser/Bindable.h"
37#include "RenderFuncs.h"
38
39#include "../vrml_parser/Structs.h"
40#include "../main/headers.h"
41
42#include "LinearAlgebra.h"
43#ifdef HAVE_OPENCL
44#include "../opencl/OpenCL_Utils.h"
45#endif //HAVE_OPENCL
46#include "Collision.h"
47#include "../internal.h"
48static struct point_XYZ get_poly_min_disp_with_sphere(double r, struct point_XYZ* p, int num, struct point_XYZ n);
49
50static struct point_XYZ weighted_sum(struct point_XYZ p1, struct point_XYZ p2, double k);
51static struct point_XYZ get_point_disp(double y1, double y2, double ystep, double r, struct point_XYZ p1, struct point_XYZ n);
52void accumulateFallingClimbing(double y1, double y2, double ystep, struct point_XYZ *p, int num, struct point_XYZ nused, double *tmin, double *tmax);
53
54
55
56
57#define DJ_KEEP_COMPILER_WARNING 0
58
59//static int dug9debug = 0;
60//int setdug9debug()
61//{ dug9debug = 1; return 0;}
62//int cleardug9debug()
63//{ dug9debug = 0; return 0;}
64
65#define swap(x,y) {double k = x; x = y; y = k; }
66#define FLOAT_TOLERANCE 0.00000001
67#if DJ_KEEP_COMPILER_WARNING
68#define MAX_POLYREP_DISP_RECURSION_COUNT 10
69#endif
70#define STEPUP_MAXINCLINE 0.9
71
72#ifdef DEBUGPTS
73#define DEBUGPTSPRINT(x,y,z) printf(x,y,z)
74#else
75#define DEBUGPTSPRINT(x,y,z) {}
76#endif
77
78static const struct point_XYZ zero = {0,0,0};
79
80typedef struct pcollision{
81 float* prd_newc_floats;// = NULL;
82 unsigned int prd_newc_floats_size;// = 0;
83 struct point_XYZ* prd_normals;// = NULL;
84 int prd_normals_size;// = 0;
85 struct point_XYZ* clippedPoly1;// = NULL;
86 int clippedPoly1Size;// = 0; /* number of struct point_XYZ* 's in the clippedPoly data area */
87 struct point_XYZ* clippedPoly2;// = NULL;
88 int clippedPoly2Size;// = 0; /* number of struct point_XYZ* 's in the clippedPoly data area */
89 struct point_XYZ* clippedPoly3;// = NULL;
90 int clippedPoly3Size;// = 0; /* number of struct point_XYZ* 's in the clippedPoly data area */
91 struct point_XYZ* clippedPoly4;// = NULL;
92 int clippedPoly4Size;// = 0; /* number of struct point_XYZ* 's in the clippedPoly data area */
93 struct point_XYZ* clippedPoly5;// = NULL;
94 int clippedPoly5Size;// = 0; /* number of struct point_XYZ* 's in the clippedPoly data area */
95
96
97 /* JAS - make return val global, not local for polyrep-disp */
98 #ifdef HAVE_OPENCL
99 struct sCollisionGPU CollisionGPU;
100 #endif
101
102
103 /* Collision detection results */
104 struct point_XYZ res;
105 double get_poly_mindisp;
106 struct sCollisionInfo CollisionInfo;// = { {0,0,0} , 0, 0. };
107 struct sFallInfo FallInfo; /* = {100.0,1.0,0.0,0.0, 0,1,0,0}; ... too many to initialize here */
108
109 /* did the OpenCL GPU Collision compile ok? */
110 bool OpenCL_Collision_Program_initialized;
111
112}* ppcollision;
113void *collision_constructor(){
114 void *v = MALLOCV(sizeof(struct pcollision));
115 memset(v,0,sizeof(struct pcollision));
116 return v;
117}
118void collision_init(struct tcollision *t){
119 //public
120 //private
121
122 t->prv = collision_constructor();
123 {
124 ppcollision p = (ppcollision)t->prv;
125 p->prd_newc_floats = NULL;
126 p->prd_newc_floats_size = 0;
127 p->prd_normals = NULL;
128 p->prd_normals_size = 0;
129 p->clippedPoly1 = NULL;
130 p->clippedPoly1Size = 0; /* number of struct point_XYZ* 's in the clippedPoly data area */
131 p->clippedPoly2 = NULL;
132 p->clippedPoly2Size = 0; /* number of struct point_XYZ* 's in the clippedPoly data area */
133 p->clippedPoly3 = NULL;
134 p->clippedPoly3Size = 0; /* number of struct point_XYZ* 's in the clippedPoly data area */
135 p->clippedPoly4 = NULL;
136 p->clippedPoly4Size = 0; /* number of struct point_XYZ* 's in the clippedPoly data area */
137 p->clippedPoly5 = NULL;
138 p->clippedPoly5Size = 0; /* number of struct point_XYZ* 's in the clippedPoly data area */
139
140
141 /* Collision detection results */
142 p->CollisionInfo.Count = 0;
143 p->CollisionInfo.Maximum2 = 0.0;
144 p->CollisionInfo.Offset.x = 0.0;
145 p->CollisionInfo.Offset.y = 0.0;
146 p->CollisionInfo.Offset.z = 0.0;
147 //p->FallInfo; /* = {100.0,1.0,0.0,0.0, 0,1,0,0}; ... too many to initialize here */
148
149 #ifdef HAVE_OPENCL
150 p->CollisionGPU.CollideGPU_program = NULL;
151 p->CollisionGPU.CollideGPU_kernel = NULL;
152 p->CollisionGPU.CollideGPU_output_buffer = NULL;
153 p->CollisionGPU.CollideGPU_matrix_buffer = NULL;
154 p->CollisionGPU.CollideGPU_vertex_buffer = NULL;
155 p->CollisionGPU.CollideGPU_index_buffer = NULL;
156 p->CollisionGPU.CollideGPU_returnValues.n = 0;
157 p->CollisionGPU.CollideGPU_returnValues.p = NULL;
158
159 p->OpenCL_Collision_Program_initialized = FALSE;
160
161 #endif
162 }
163}
164void collision_clear(struct tcollision *t){
165 //public
166 //private
167 {
168 ppcollision p = (ppcollision)t->prv;
169 FREE_IF_NZ(p->prd_newc_floats);
170 FREE_IF_NZ(p->prd_normals);
171 }
172}
173
174// ppcollision p = (ppcollision)gglobal()->collision.prv;
175struct sCollisionInfo* CollisionInfo()
176{
177 ppcollision p = (ppcollision)gglobal()->collision.prv;
178 return &p->CollisionInfo;
179}
180struct sFallInfo* FallInfo()
181{
182 ppcollision p = (ppcollision)gglobal()->collision.prv;
183 return &p->FallInfo;
184}
185
186
187
188#ifdef HAVE_OPENCL
189// compile the collision gpu program here, with the local structures.
190void createGPUCollisionProgram () {
191
192 ppcollision p = (ppcollision)gglobal()->collision.prv;
193 p->OpenCL_Collision_Program_initialized = collision_initGPUCollide(&p->CollisionGPU);
194}
195
196struct sCollisionGPU* GPUCollisionInfo()
197{
198 ppcollision p = (ppcollision)gglobal()->collision.prv;
199 return &p->CollisionGPU;
200}
201#endif // HAVE_OPENCL
202
203/*a constructor */
204#define make_pt(p,xc,yc,zc) { p.x = (xc); p.y = (yc); p.z = (zc); }
205
206int overlapMBBs(GLDOUBLE *MBBmin1, GLDOUBLE *MBBmax1, GLDOUBLE *MBBmin2, GLDOUBLE* MBBmax2)
207{
208 /* test for overlap between two axes aligned minimum bounding boxes MBBs in same space.
209 returns true if they overlap
210 rule: must overlap in all 3 dimensions in order to intersect
211 dimension your MBB: double MBBmin[3], MBBmax[3];
212 */
213
214 int i, overlap;
215 overlap = TRUE;
216 for(i=0;i<3;i++)
217 {
218 overlap = overlap && !(MBBmin1[i] > MBBmax2[i] || MBBmax1[i] < MBBmin2[i]);
219 }
220 return overlap;
221}
222
223
224
225/*accumulator function, for displacements. */
226void accumulate_disp(struct sCollisionInfo* ci, struct point_XYZ add) {
227 double len2 = vecdot(&add,&add);
228 ci->Count++;
229 VECADD(ci->Offset,add);
230 if(len2 > ci->Maximum2)
231 ci->Maximum2 = len2;
232}
233
234static double closest_point_of_segment_to_origin(struct point_XYZ p1, struct point_XYZ p2) {
235 /*the equation (guessed from above)*/
236 double x12 = (p1.x - p2.x);
237 double y12 = (p1.y - p2.y);
238 double z12 = (p1.z - p2.z);
239 double q = ( x12*x12 + y12*y12 + z12*z12 );
240
241 /* double i = ((q == 0.) ? 0.5 : (p1.x * x12 + p1.y * y12 + p1.z * z12) / q); */
242 double i = ((APPROX(q, 0)) ? 0.5 : (p1.x * x12 + p1.y * y12 + p1.z * z12) / q);
243
244 /* struct point_XYZ result; */
245
246 /*clamp result to constraints */
247 if(i < 0) i = 0.;
248 if(i > 1) i = 1.;
249
250 return i;
251
252}
253
254/*n must be normal */
255static struct point_XYZ closest_point_of_plane_to_origin(struct point_XYZ b, struct point_XYZ n) {
256 /*the equation*/
257 double k = b.x*n.x + b.y*n.y + b.z*n.z;
258 vecscale(&n,&n,k);
259 return n;
260}
261
262
263/* [p1,p2[ is segment, q1,q2 defines line */
264/* ignores y coord. eg intersection is done on projection of segment and line on the y plane */
265/* nowtice point p2 is NOT included, (for simplification elsewhere) */
266
267static int intersect_segment_with_line_on_yplane(struct point_XYZ* pk, struct point_XYZ p1, struct point_XYZ p2, struct point_XYZ q1, struct point_XYZ q2) {
268 double k,quotient;
269 //double l;
270
271 /* p2 becomes offset */
272 VECDIFF(p2,p1,p2);
273 /* q2 becomes offset */
274 VECDIFF(q2,q1,q2);
275
276 /* if(q2.x == 0 && q2.z == 0.) */
277 if(APPROX(q2.x, 0) && APPROX(q2.z, 0)) {
278 /* degenerate case.
279 it fits our objective to simply specify a random line. */
280 q2.x = 1;
281 q2.y = 0;
282 q2.z = 0;
283 }
284
285 quotient = ((-p2.z)*q2.x + p2.x*q2.z);
286 /* if(quotient == 0.) return 0; */
287 if(APPROX(quotient, 0)) return 0;
288
289 k = (p1.z*q2.x - q1.z*q2.x - p1.x*q2.z + q1.x*q2.z)/quotient;
290 //l = (p1.z*p2.x - p1.x*p2.z + p2.z*q1.x - p2.x*q1.z)/quotient;
291
292 if((k >= 0.) && (k < 1.)) {
293 vecscale(pk,&p2,k);
294 VECADD(*pk,p1);
295 return 1;
296 } else
297 return 0;
298
299}
300
301/*finds the intersection of the line pp1 + k n with a cylinder on the y axis.
302 returns the 0,1 or 2 values.
303 */
304static int getk_intersect_line_with_ycylinder(double* k1, double* k2, double r, struct point_XYZ pp1, struct point_XYZ n) {
305 double b,a,sqrdelta,delta;
306 /* int res = 0; */
307
308 /*solves (pp1+ k n) . (pp1 + k n) = r^2 , ignoring y values.*/
309 a = 2*(n.x*n.x + n.z*n.z);
310 b = -2*(pp1.x*n.x + pp1.z*n.z);
311 delta = (4*((pp1.x*n.x + pp1.z*n.z)*(pp1.x*n.x + pp1.z*n.z)) -
312 4*((n.x*n.x + n.z*n.z))*((pp1.x*pp1.x + pp1.z*pp1.z - r*r)));
313 if(delta < 0.) return 0;
314 sqrdelta = sqrt(delta);
315
316 *k1 = (b+sqrdelta)/a;
317 /* if(sqrdelta == 0.) return 1; */
318 if(APPROX(sqrdelta, 0)) return 1;
319
320 *k2 = (b-sqrdelta)/a;
321 return 2;
322}
323
324
325/*projects a point on the y="y" plane, in the direction of n. *
326 n probably needs to be normal. */
327static struct point_XYZ project_on_yplane(struct point_XYZ p1, struct point_XYZ n,double y) {
328 struct point_XYZ ret;
329 make_pt(ret,p1.x - (n.x*(p1.y-y))/n.y,y,(p1.z - (n.z*(p1.y-y))/n.y));
330 return ret;
331}
332
333/*projects a point on the plane tangent to the surface of the cylinder at point -kn (the prolonged normal)
334 , in the inverse direction of n.
335 n probably needs to be normal. */
336static struct point_XYZ project_on_cylindersurface_plane(struct point_XYZ p, struct point_XYZ n,double r) {
337 struct point_XYZ pp;
338 struct point_XYZ ret;
339 vecscale(&n,&n,-1.0);
340 pp = n;
341 pp.y = 0;
342 vecnormal(&pp,&pp);
343 vecscale(&pp,&pp,r);
344
345 ret.x = p.x + (n.x*((n.x*((pp.x - p.x)) + n.z*((pp.z - p.z)))))/(n.x*n.x + n.z*n.z);
346 ret.y = p.y + (n.y*((n.x*((pp.x - p.x)) + n.z*((pp.z - p.z)))))/(n.x*n.x + n.z*n.z);
347 ret.z = p.z + (n.z*((n.x*((pp.x - p.x)) + n.z*((pp.z - p.z)))))/(n.x*n.x + n.z*n.z);
348
349 return ret;
350}
351
352/*makes half-plane starting at point, perpendicular to plane (eg: passing through origin)
353 if this plane cuts through polygon edges an odd number of time, we are inside polygon*/
354/* works for line passing through origin, polygon plane must not pass through origin. */
355static int perpendicular_line_passing_inside_poly(struct point_XYZ a,struct point_XYZ* p, int num) {
356 struct point_XYZ n; /*half-plane will be defined as: */
357 struct point_XYZ i; /* p(x,y) = xn + yi, with i >= 0 */
358 struct point_XYZ j; /* j is half-plane normal */
359 int f,sectcount = 0;
360 struct point_XYZ epsilon; /* computationnal trick to handle points directly on plane. displace them. */
361
362//printf ("\t\t\tperpendicular_line_passing_inside_poly, num %d\n",num);
363
364 /* if(vecnormal(&n,&a) == 0) */
365 if(APPROX(vecnormal(&n,&a), 0)) {
366 /* happens when polygon plane passes through origin */
367 return 0;
368 }
369 make_orthogonal_vector_space(&i,&j,n);
370
371 vecscale(&epsilon,&j,FLOAT_TOLERANCE); /*make our epsilon*/
372
373 for(f = 0; f < num; f++) {
374 /*segment points relative to point a */
375 struct point_XYZ p1,p2;
376 double p1j,p2j;
377 VECDIFF(p[f],a,p1);
378 VECDIFF(p[(f+1)%num],a,p2);
379 /* while((p1j = vecdot(&p1,&j)) == 0.) VECADD(p1,epsilon); */
380 while(APPROX((p1j = vecdot(&p1,&j)), 0)) VECADD(p1,epsilon);
381
382 /* while((p2j = vecdot(&p2,&j)) == 0.) VECADD(p2,epsilon); */
383 while(APPROX((p2j = vecdot(&p2,&j)), 0)) VECADD(p2,epsilon);
384
385 /*see if segment crosses plane*/
386 if(p1j * p2j <= 0 /*if signs differ*/) {
387 double k;
388 struct point_XYZ p0;
389 /* solves (k p1 + (1 - k)p2).j = 0 */
390 /* k = (p1j-p2j != 0) ? (p1j/ (p1j - p2j)) : 0.; */
391 k = (! APPROX(p1j-p2j, 0)) ? (p1j/ (p1j - p2j)) : 0.;
392
393 /*see if point on segment that is on the plane (p0), is also on the half-plane */
394 p0 = weighted_sum(p1, p2, k);
395 if(vecdot(&p0,&i) >= 0)
396 sectcount++;
397 }
398 }
399
400//printf ("\t\t\tend perpendicular_line_passing_inside_poly, ret %d\n",sectcount % 2);
401 return sectcount % 2;
402
403}
404
405
406/*finds the intersection of the segment(pp1,pp2) with a cylinder on the y axis.
407 returns the 0,1 or 2 values in the range [0..1]
408 */
409static int getk_intersect_segment_with_ycylinder(double* k1, double* k2, double r, struct point_XYZ pp1, struct point_XYZ pp2) {
410 double b,a,sqrdelta,delta;
411 int res = 0;
412
413 /* pp2 becomes offset */
414 VECDIFF(pp2,pp1,pp2);
415
416 /*solves (pp1+ k pp2) . (pp1 + k pp2) = r^2 */
417 a = 2*(pp2.x*pp2.x + pp2.z*pp2.z);
418 b = -2*(pp1.x*pp2.x + pp1.z*pp2.z);
419 delta = (4*((pp1.x*pp2.x + pp1.z*pp2.z)*(pp1.x*pp2.x + pp1.z*pp2.z)) -
420 4*((pp2.x*pp2.x + pp2.z*pp2.z))*((pp1.x*pp1.x + pp1.z*pp1.z - r*r)));
421 if(delta < 0.) return 0;
422 sqrdelta = sqrt(delta);
423 /* keep the ks that are in the segment */
424 *k1 = (b+sqrdelta)/a;
425 *k2 = (b-sqrdelta)/a;
426
427 if(*k1 >= 0. && *k1 <= 1.) res++;
428 if(*k2 >= 0. && *k2 <= 1.) res++;
429 if(res == 1 && (*k1 < 0. || *k1 > 1.)) swap(*k1,*k2);
430/* if(res == 2 && sqrdelta == 0.) res = 1; */
431
432 return res;
433}
434
435/*returns (1-k)p1 + k p2 */
436static struct point_XYZ weighted_sum(struct point_XYZ p1, struct point_XYZ p2, double k) {
437 struct point_XYZ ret;
438 make_pt(ret,
439 p1.x*(1-k)+p2.x*k,
440 p1.y*(1-k)+p2.y*k,
441 p1.z*(1-k)+p2.z*k);
442
443 /* printf ("weighted sum, from %lf %lf %lf, %lf %lf %lf, return %lf %lf %lf\n",
444 p1.x,p1.y,p1.z,p2.x,p2.y,p2.z,ret.x,ret.y,ret.z); */
445 return ret;
446}
447
448
449static int pointOnPlaneInsidePoly(struct point_XYZ D,struct point_XYZ *p, int num, struct point_XYZ* n )
450{
451 int i,j,inside;
452
453 struct point_XYZ a, b,c,last; //,d,e;
454
455 inside = 1;
456 for(i=0;i<num;i++)
457 {
458 j = (i+1)%num; //i==(num-1)? 0 : i + 1;
459 vecdiff(&a,&D,&p[i]);
460 vecdiff(&b,&p[j],&p[i]);
461 veccross(&c,a,b);
462 if( i > 0 )
463 inside = inside && vecdot(&last,&c) >= 0.0;
464 last = c;
465 }
466 return inside;
467}
468
469static int intersectLineSegmentWithPoly(struct point_XYZ s0,struct point_XYZ s1,double r, struct point_XYZ *p,int num,struct point_XYZ n,double *dr)
470{
471 /* returns 1 if a line segment intersects a convex planar polygon, else 0
472 if 1, returns dr the fraction of D=(s1-s0) where the intersection point is
473 so to get the intersection point from dr, go (s1-s0)*dr + s0 in your calling code
474 tested use: send in s1 normalized (length 1), s0=0,0,0 and r the length you want the segment
475 see p.391 of Graphics Gems I (a book)
476 line segment:
477 s0 - starting point of ray
478 s1 - direction vector of ray unit length
479 r - scalar length of ray
480 polygon:
481 p[num] - points
482 n - normal (precomputed)
483 return:
484 0 - no intersection
485 1 - intersection within poly and within line segment
486 dr - intersection point expressed as fraction of (s1-s0) in range 0 to r
487 */
488
489 int hit = 0;
490 double d,t, ndotD;
491 struct point_XYZ D;
492 /* step1 intersect infinite line with infinite plane */
493 d = -vecdot(&n,&p[0]); /* compute plane constant */
494 VECDIFF(s1,s0,D); /* compute ray direction vector from line segment start and end points */
495 /* vecnormal(&D,&D); D should already be normalized - just sin & cos values */
496 ndotD = vecdot(&n,&D);
497 *dr = 0.0; /* displacement inward from r */
498 if(APPROX(ndotD,0.0) )
499 {
500 /* infinite plane and infinite line are parallel - no intersection */
501 *dr = 0.0;
502 return hit;
503 }
504 t = - ( (d + vecdot(&n,&s0)) / ndotD ); /*magic plane-line intersection equation - t is parametric value of intersection point on line */
505 /* step2 test intersection in line segment */
506 if( t < 0.0 || t > r )
507 {
508 /* intersection is outside of line segment */
509 return hit;
510 }
511
512 /* step3 test intersection in poly */
513 vecscale(&D,&D,t);
514 VECADD(D,s0);
515 hit = pointOnPlaneInsidePoly(D,p,num,&n);
516 if( hit ) *dr = t;
517 return hit;
518}
519
520/*feed a poly, and stats of a cylinder, it returns the displacement in the radial direction of the
521 avatar that is needed for them not to intersect any more */
522static struct point_XYZ get_poly_radialSample_disp(double y1, double y2, double ystep, double r,struct point_XYZ* p, int num, struct point_XYZ n, double *tmin, double *tmax)
523{
524 /* uses a statistical sampler method - 8 radial rays at the ystep, avatar origin Y=0, and y2, levels, each ray segment r long
525 It's a sampler because it will miss small things. But it is supposed to catch major things like walls.
526 */
527 int i,j,hit;
528 double level[3],dr,dmax,eighth,theta, avmin[3], avmax[3];
529 struct point_XYZ s0,s1,result = zero;
530 level[0] = ystep;
531 level[1] = 0.0;
532 level[2] = y2;
533 s0.x = s0.y = s0.z = 0.0; /*avatar axis*/
534 dmax = 0.0;
535 eighth = M_PI * 2.0 / 8.0;
536
537 for(i=0;i<3;i++)
538 {
539 avmin[i] = -r;
540 avmax[i] = r;
541 }
542 avmin[1] = ystep; avmax[1] = y2;
543 if(!overlapMBBs(avmin,avmax,tmin,tmax)) return result;
544
545
546 for(i=0;i<3;i++)
547 {
548 s0.y = level[i];
549 s1.y = level[i];
550 theta = 0.0;
551 for(j=0;j<8;j++)
552 {
553 theta += eighth;
554 s1.x = cos(theta);
555 s1.z = sin(theta);
556 /* quick check of overlap */
557 avmin[0] = DOUBLE_MIN(s0.x,s1.x);
558 avmin[1] = DOUBLE_MIN(s0.y,s1.y);
559 avmin[2] = DOUBLE_MIN(s0.z,s1.z);
560 avmax[0] = DOUBLE_MAX(s0.x,s1.x);
561 avmax[1] = DOUBLE_MAX(s0.y,s1.y);
562 avmax[2] = DOUBLE_MAX(s0.z,s1.z);
563 if( overlapMBBs(avmin,avmax,tmin,tmax) )
564 {
565 hit = intersectLineSegmentWithPoly(s0,s1,r,p,num,n,&dr);
566 if(hit)
567 {
568 if( (dr > FLOAT_TOLERANCE) && (dr > dmax) )
569 {
570 dmax = r-dr;
571 vecscale(&result,&s1,r-dr);
572 result.y = 0.0;
573 //printf(">");
574 }
575 }
576 }
577 }
578 }
579 /* process hits to find optimal direction and magnitude */
580 return result;
581}
582
583static int get_poly_penetration_disp( double r,struct point_XYZ* p, int num, struct point_XYZ n, double *tmin, double *tmax, struct point_XYZ *result, double *rmax)
584{
585 /* checks for wall penetration and computes a correction
586 checks between the convex poly you pass in,
587 and LastPos-Pos vector saved in FallInfo struct in render_collision()
588 r - avatar radius - will be added onto a penetration correction so avatar is repositioned off the wall
589 p[num] - convex planar poly to test
590 n - poly normal precomputed
591 tmin[3],tmax[3] - poly MBB precomputed
592 returns:
593 zero (0,0,0) if no intersection else
594 result - the displacement vector needed to move the avatar back
595 all coordinates in collision space
596 */
597 int hit = 0;
598 double dr;
599 struct sFallInfo *fi;
600 struct point_XYZ s0=zero;
601 *result = zero;
602 *rmax = 0.0;
603 fi = FallInfo();
604
605 /* from FallInfo struct:
606 penvec - unit vector in direction from avatar (0,0,0) toward the last avatar position on the last loop (or more precisely, last time checked)
607 penRadius - distance between avatar(0,0,0) and last avatar position 0+ - infinity
608 penMin[3],penMax[3] - pre-computed MBB of penvec
609 all coordinates in collision space
610 */
611 if( overlapMBBs(fi->penMin,fi->penMax,tmin,tmax) )
612 {
613 /* penvec length 1.0 normalized. penRadius can be 0 to 1000+ - how far did you travel/navigate on one loop */
614 hit = intersectLineSegmentWithPoly(s0,fi->penvec,fi->penRadius,p,num,n,&dr);
615 if(hit)
616 {
617 vecscale(result,&fi->penvec,(dr + r));
618 *rmax = dr;
619 }
620 }
621 return hit;
622}
623
624struct point_XYZ get_poly_disp_2(struct point_XYZ* p, int num, struct point_XYZ n) {
625
626 /*
627 generalized logic for all planar facet geometry and avatar collision volumes
628 If you can break your geometry into planar facets, then call this for each facet.
629 We will know what to do here based on global structs.
630
631 The avatar collision volume -
632 A.for flying/examining:
633 1. sphere.
634
635 B.For walking:
636 1. cylinder (body) from (-Height + stepSize) to (+avatarWidth)
637 2. line - climb/fall (legs,feet) from (-FallInfo.FallHeight to 0)
638 3. ray to last avatar position for wall penetration
639 Order of tests (so can exit early to save unnecessary computations):
640 a) ray b) cylinder c) line
641 If you have a wall penetration ray hit, you don't need to check for cylinder collision.
642 If you have a cylinder hit, you don't need to test climb/fall.
643 if you have a climb, you can skip the fall.
644 otherwise test for fall.
645
646 fast tests using MBB in the caller elliminate some volumes by setting check variables to true if MBB overlap
647 checkPenetration - ray for wall penetration should be tested
648 checkCylinder - sphere or cyclinder should be tested
649 checkFall - climb and fall should be tested
650 */
651
652 struct point_XYZ result;
653 int hit,i;
654 double tmin[3],tmax[3]; /* MBB for facet */
655 struct sFallInfo *fi;
656 struct sNaviInfo *naviinfo;
657 GLDOUBLE awidth, atop, abottom, astep;
658 ppcollision pp;
659 ttglobal tg = gglobal();
660 naviinfo = (struct sNaviInfo *)tg->Bindable.naviinfo;
661 awidth = naviinfo->width; /*avatar width*/
662 atop = naviinfo->width; /*top of avatar (relative to eyepoint)*/
663 abottom = -naviinfo->height; /*bottom of avatar (relative to eyepoint)*/
664 astep = -naviinfo->height+naviinfo->step;
665 pp = (ppcollision)tg->collision.prv;
666
667 result = zero;
668 pp->get_poly_mindisp = 0.0;
669 fi = FallInfo();
670
671 if(fi->walking)
672 {
673 tmin[0] = tmax[0] = p[0].x;
674 tmin[1] = tmax[1] = p[0].y;
675 tmin[2] = tmax[2] = p[0].z;
676 for(i=1;i<num;i++)
677 {
678 tmin[0] = DOUBLE_MIN(tmin[0],p[i].x);
679 tmin[1] = DOUBLE_MIN(tmin[1],p[i].y);
680 tmin[2] = DOUBLE_MIN(tmin[2],p[i].z);
681 tmax[0] = DOUBLE_MAX(tmax[0],p[i].x);
682 tmax[1] = DOUBLE_MAX(tmax[1],p[i].y);
683 tmax[2] = DOUBLE_MAX(tmax[2],p[i].z);
684 }
685
686// printf ("get_poly_disp_2, checkPenetration %d checkCylinder %d checkFall %d true %d\n",fi->checkPenetration, fi->checkCylinder, fi->checkFall, TRUE);
687
688 /* walking */
689 hit = 0;
690 if(fi->checkPenetration)
691 {
692 double rmax;
693 struct point_XYZ presult;
694 hit = get_poly_penetration_disp(awidth,p,num,n,tmin,tmax,&presult,&rmax);
695 if(hit)
696 {
697 fi->isPenetrate = 1;
698 if(rmax > fi->pendisp)
699 {
700 fi->pencorrection = presult;
701 fi->pendisp = rmax;
702 }
703 }
704 }
705 if(fi->checkCylinder && !hit)
706 {
707 result = get_poly_radialSample_disp(abottom,atop,astep,awidth,p,num,n,tmin,tmax); //statistical method
708
709 hit = !(APPROX(result.x, 0) && APPROX(result.y, 0) && APPROX(result.z, 0));
710 if(hit)
711 fi->canFall = 0; /* rule: don't fall or climb if we are colliding */
712 }
713 if(fi->checkFall && !hit)
714 {
715 accumulateFallingClimbing(abottom,atop,astep,p,num,n,tmin,tmax); //y1, y2, p, num, n);
716
717 }
718 }
719 else
720 {
721 /* fly, examine */
722 // printf ("calling get_poly_min_disp_with_sphere at %s:%d\n",__FILE__,__LINE__);
723 result = get_poly_min_disp_with_sphere(awidth, p, num, n);
724 }
725 pp->get_poly_mindisp = vecdot(&result,&result);
726 // printf ("\tend get_poly_disp_2, result %lf %lf %lf\n",result.x,result.y,result.z);
727 return result;
728}
729
730
731/*feed a poly, and radius of a sphere, it returns the minimum displacement and
732 the direction that is needed for them not to intersect any more.
733*/
734static struct point_XYZ get_poly_min_disp_with_sphere(double r, struct point_XYZ* p, int num, struct point_XYZ n) {
735 int i,j;
736 /* double polydisp; */
737 struct point_XYZ result;
738 double tmin[3],tmax[3],rmin[3],rmax[3],q[3];
739 double get_poly_mindisp;
740 int clippedPoly4num = 0;
741 ppcollision pp = (ppcollision)gglobal()->collision.prv;
742 get_poly_mindisp = 1E90;
743
744 /* printf ("\nstart of get_poly_min_disp_with_sphere\n"); */
745
746 /* cheap MBB test */
747 //double tmin[3],tmax[3],rmin[3],rmax[3],q[3];
748
749 /* initialize the point array to something... */
750 memcpy(tmin,&p[0],3*sizeof(double));
751 memcpy(tmax,&p[num-1],3*sizeof(double));
752
753 for(i=0;i<num;i++)
754 {
755 memcpy(q,&p[i],3*sizeof(double));
756 for(j=0;j<3;j++)
757 {
758 tmin[j] = DOUBLE_MIN(tmin[j],q[j]);
759 tmax[j] = DOUBLE_MAX(tmax[j],q[j]);
760 }
761 }
762 for(i=0;i<3;i++)
763 {
764 rmin[i] = -r;
765 rmax[i] = r;
766 }
767
768 if( !overlapMBBs(rmin,rmax,tmin,tmax) )
769 {
770 return zero;
771 }
772 /* end cheap MBB test */
773
774#ifdef DEBUGFACEMASK
775 if(facemask != debugsurface++)
776 return zero;
777#endif
778 /*allocate data */
779 if ((num+1)> pp->clippedPoly4Size) {
780 pp->clippedPoly4 = (struct point_XYZ*) REALLOC(pp->clippedPoly4,sizeof(struct point_XYZ) * (num + 1));
781 pp->clippedPoly4Size = num+1;
782 }
783
784 /*if normal not specified, calculate it */
785 /* if(n.x == 0 && n.y == 0 && n.z == 0) */
786 if(APPROX(n.x, 0) && APPROX(n.y, 0) && APPROX(n.z, 0)) {
787 polynormal(&n,&p[0],&p[1],&p[2]);
788 }
789
790
791 for(i = 0; i < num; i++) {
792 DEBUGPTSPRINT("intersect_closestpolypoints_on_surface[%d]= %d\n",i,clippedPoly4num);
793 pp->clippedPoly4[clippedPoly4num++] = weighted_sum(p[i],p[(i+1)%num],closest_point_of_segment_to_origin(p[i],p[(i+1)%num]));
794 }
795
796 /*find closest point of polygon plane*/
797 pp->clippedPoly4[clippedPoly4num] = closest_point_of_plane_to_origin(p[0],n);
798
799
800/* {
801int m;
802printf ("\t\tget_poly_min_disp_with_sphere, clippedPoly4 array\n");
803for (m=0; m<=clippedPoly4num; m++)
804printf ("\t\t\tclippedPoly4 %d is %f %f %f\n",m,pp->clippedPoly4[m].x, pp->clippedPoly4[m].y, pp->clippedPoly4[m].z);
805} */
806
807
808
809
810 /*keep if inside*/
811 if(perpendicular_line_passing_inside_poly(pp->clippedPoly4[clippedPoly4num],p, num)) {
812 DEBUGPTSPRINT("perpendicular_line_passing_inside_poly[%d]= %d\n",0,clippedPoly4num);
813 clippedPoly4num++;
814 }
815
816#ifdef DEBUGPTS
817 for(i=0; i < clippedPoly4num; i++) {
818 debugpts.push_back(clippedPoly4[i]);
819 }
820#endif
821
822 /*here we find mimimum displacement possible */
823
824 /*calculate the closest point to origin */
825//printf ("\t\tget_poly_min_disp_with_sphere, clippedPoly4num %d\n",clippedPoly4num);
826 for(i = 0; i < clippedPoly4num; i++)
827 {
828 /* printf ("\t\tget_poly_min_disp_with_sphere, checking against %d %f %f %f",i,pp->clippedPoly4[i].x,
829 pp->clippedPoly4[i].y,pp->clippedPoly4[i].z); */
830
831 double disp = vecdot(&pp->clippedPoly4[i],&pp->clippedPoly4[i]);
832
833 /* printf (" disp %lf, get_poly_mindisp %lf\n",disp,get_poly_mindisp); */
834
835 if(disp < get_poly_mindisp)
836 {
837 get_poly_mindisp = disp;
838 result = pp->clippedPoly4[i];
839 }
840 }
841 /* printf ("SW, get_poly_min_disp_with_sphere way, get_poly_mindisp %f\n",get_poly_mindisp); */
842
843 if(get_poly_mindisp <= r*r)
844 {
845 double rl;
846
847 /* printf ("\t\tWOW!!! WOW!!! get_poly_min_disp_with_sphere, less than r*r\n");
848 printf ("have to move, have poly_mindisp %f, r*r %f, point %f %f %f\n",get_poly_mindisp, r*r, result.x,result.y,result.z);
849 */
850
851
852 /* scale result to length of missing distance. */
853 rl = veclength(result);
854 /* printf ("get_poly_min_disp_with_sphere, comparing %f and %f veclen %lf result %f %f %f\n",get_poly_mindisp, r*r, rl, result.x,result.y,result.z); */
855 /* if(rl != 0.) */
856 if(! APPROX(rl, 0))
857 {
858 /* printf ("approx rl, 0... scaling by %lf, %lf - %lf / %lf\n",(r-sqrt(get_poly_mindisp)) / rl,
859 r, sqrt(get_poly_mindisp), rl); */
860 vecscale(&result,&result,(r-sqrt(get_poly_mindisp)) / rl);
861
862 /* printf ("by the non-OpenCL software way, result %f %f %f\n",result.x, result.y, result.z); */
863 }
864 else
865 {
866 result = zero;
867 }
868 }
869 else
870 {
871 result = zero;
872 }
873 // printf ("\t\tend get_poly_min_disp_with_sphere result %lf %lf %lf\n",result.x, result.y, result.z);
874 return result;
875}
876
877
878/*used by get_line_normal_disp to clip the polygon on the cylinder caps, called twice*/
879static int helper_line_clip_cap(struct point_XYZ* clippedpoly, int clippedpolynum, struct point_XYZ p1, struct point_XYZ p2, double r, struct point_XYZ n, double y, int stepping)
880{
881 struct point_XYZ ppoly[2];
882 int allin = 1;
883 int i;
884
885 if(!stepping) {
886 /*sqush poly on cylinder cap plane.*/
887 ppoly[0] = project_on_yplane(p1,n,y);
888 ppoly[1] = project_on_yplane(p2,n,y);
889 } else {
890 ppoly[0] = p1;
891 ppoly[1] = p2;
892 }
893
894 /*find points of poly hitting cylinder cap*/
895 for(i= 0; i < 2; i++) {
896 if(ppoly[i].x*ppoly[i].x + ppoly[i].z*ppoly[i].z > r*r) {
897 allin = 0;
898 } else {
899 clippedpoly[clippedpolynum++] = ppoly[i];
900 }
901 }
902
903 if(!allin) {
904 /* int numdessect = 0; */
905 struct point_XYZ dessect;
906 double k1,k2;
907 int nsect;
908
909
910 /*find intersections of line with cylinder cap edge*/
911 nsect = getk_intersect_segment_with_ycylinder(&k1,&k2,r,ppoly[0],ppoly[1]);
912 switch(nsect) {
913 case 2:
914 if(fabs(k1-k2) < FLOAT_TOLERANCE) /* segment touches edge of circle. we want to ignore this. */
915 break;
916 clippedpoly[clippedpolynum++] = weighted_sum(ppoly[0],ppoly[1],k2);
917 case 1:
918 clippedpoly[clippedpolynum++] = weighted_sum(ppoly[0],ppoly[1],k1);
919 case 0: break;
920 }
921 /*find intersections of descending segment too.
922 these will point out maximum and minimum in cylinder cap edge that is inside triangle */
923 if(intersect_segment_with_line_on_yplane(&dessect,ppoly[0],ppoly[1],n,zero)) {
924 if(dessect.x*dessect.x + dessect.z*dessect.z < r*r) {
925
926 clippedpoly[clippedpolynum++] = dessect;
927 }
928 }
929 }
930
931 return clippedpolynum;
932}
933
934/*feed a line and a normal, and stats of a cylinder, it returns the displacement in the direction of the
935 normal that is needed for them not to intersect any more.*/
936static struct point_XYZ get_line_normal_disp(double y1, double y2, double r, struct point_XYZ p1, struct point_XYZ p2, struct point_XYZ n) {
937 int i;
938 double mindisp = 0;
939 double polydisp;
940 struct point_XYZ p[2];
941 int num = 2;
942 struct point_XYZ result;
943
944 struct point_XYZ clippedpoly[14];
945 int clippedpolynum = 0;
946
947 p[0] = p1;
948 p[1] = p2;
949
950 /* clip line on top and bottom cap */
951 /* if(n.y!= 0.) */
952 if(! APPROX(n.y, 0)) {
953 clippedpolynum = helper_line_clip_cap(clippedpoly, clippedpolynum, p1, p2, r, n, y1, 0);
954 clippedpolynum = helper_line_clip_cap(clippedpoly, clippedpolynum, p1, p2, r, n, y2, 0);
955 }
956
957 /*find intersections of line with cylinder side*/
958 /* if(n.y != 1. && n.y != -1.) */ /*n.y == +/-1 makes n.x == n.z == 0, wich does div's by 0, besides making no sense at all. */
959 if(! APPROX(n.y, 1) && ! APPROX(n.y, -1)) { /*n.y == +/-1 makes n.x == n.z == 0, wich does div's by 0, besides making no sense at all. */
960
961 struct point_XYZ dessect3d;
962 double k1,k2;
963 int nsect;
964
965
966 /*find points of poly intersecting descending line on poly, (non-projected)*/
967 if(intersect_segment_with_line_on_yplane(&dessect3d,p[0],p[1],n,zero)) {
968 dessect3d = project_on_cylindersurface_plane(dessect3d,n,r);
969
970 if(dessect3d.y < y2 &&
971 dessect3d.y > y1)
972 clippedpoly[clippedpolynum++] = dessect3d;
973
974 }
975 { /*find intersections on cylinder of polygon points projected on surface */
976 struct point_XYZ sect[2];
977 for(i = 0; i < num; i++) {
978 nsect = getk_intersect_line_with_ycylinder(&k1, &k2, r, p[i], n);
979 if(nsect == 0) continue;
980
981 /*sect = p[i] + k2 n*/
982 vecscale(&sect[i],&n,k2);
983 VECADD(sect[i],p[i]);
984
985 if(sect[i].y > y1 && sect[i].y < y2) {
986 clippedpoly[clippedpolynum++] = sect[i];
987 }
988 }
989 /*case where vertical line passes through cylinder, but no edges are inside */
990 /* if( (n.y == 0.) && ( */
991 if( (APPROX(n.y, 0)) && (
992 (sect[0].y <= y1 && sect[1].y >= y2) ||
993 (sect[1].y <= y1 && sect[0].y >= y2) )) {
994 sect[0].y = (y1+y2)/2;
995 clippedpoly[clippedpolynum++] = sect[0];
996 }
997 }
998
999 }
1000
1001#ifdef DEBUGPTS
1002 for(i=0; i < clippedpolynum; i++) {
1003 debugpts.push_back(clippedpoly[i]);
1004 }
1005#endif
1006
1007 /*here we find mimimum displacement possible */
1008 polydisp = vecdot(&p[0],&n);
1009
1010 /*calculate farthest point from the "n" plane passing through the origin */
1011 for(i = 0; i < clippedpolynum; i++) {
1012 double disp = vecdot(&clippedpoly[i],&n) - polydisp;
1013 if(disp < mindisp) mindisp = disp;
1014 }
1015 vecscale(&result,&n,mindisp);
1016
1017 return result;
1018}
1019
1020/*feed a line and a normal, and stats of a cylinder, it returns the vertical displacement
1021 that is needed for them not to intersect any more.*/
1022static struct point_XYZ get_line_step_disp(double y1, double y2, double r, struct point_XYZ p1, struct point_XYZ p2, struct point_XYZ n) {
1023 int i;
1024 /* int allin = 1; */
1025 double dmax = -1E99;
1026 struct point_XYZ result;
1027
1028 int clippedPoly5num = 0;
1029 ppcollision pp = (ppcollision)gglobal()->collision.prv;
1030
1031 pp->get_poly_mindisp = 1E90;
1032
1033#ifdef DEBUGFACEMASK
1034 printf("facemask = %d, debugsurface = %d\n",facemask,debugsurface);
1035 if((facemask & (1 <<debugsurface++)) ) return zero;
1036#endif
1037
1038 if((p1.y > y2 || p2.y > y2 || n.y < 0) && n.y < STEPUP_MAXINCLINE) /*to high to step on and to steep to step on or facing downwards*/
1039 return zero;
1040
1041 /*allocate data */
1042 if ((10)> pp->clippedPoly5Size) {
1043 pp->clippedPoly5 = (struct point_XYZ*) REALLOC(pp->clippedPoly5,sizeof(struct point_XYZ) * (10));
1044 pp->clippedPoly5Size = 10;
1045 }
1046
1047
1048 clippedPoly5num = helper_line_clip_cap(pp->clippedPoly5, clippedPoly5num, p1, p2, r, n, y1,1 );
1049
1050#ifdef DEBUGPTS
1051 for(i=0; i < clippedPoly5num; i++) {
1052 debugpts.push_back(clippedPoly5[i]);
1053 }
1054#endif
1055
1056 /*get maximum*/
1057 for(i = 0; i < clippedPoly5num; i++) {
1058 if(pp->clippedPoly5[i].y > dmax)
1059 dmax = pp->clippedPoly5[i].y;
1060 }
1061
1062 /*diplace only if displacement completely clears line*/
1063 if(dmax > y2)
1064 return zero;
1065
1066 pp->get_poly_mindisp = y1-dmax;
1067
1068 if(dmax > y1) {
1069 result.x = 0;
1070 result.y = pp->get_poly_mindisp;
1071 result.z = 0;
1072 return result;
1073 } else
1074 return zero;
1075}
1076
1077/* JASJASJAS */
1078
1079
1080
1081/*feed a line and a normal, and stats of a cylinder, it returns the displacement in the direction of the
1082 normal, or the vertical displacement(in case of stepping) that is needed for them not to intersect any more.*/
1083static struct point_XYZ get_line_disp(double y1, double y2, double ystep, double r, struct point_XYZ p1, struct point_XYZ p2, struct point_XYZ n) {
1084 struct point_XYZ result;
1085 result = get_line_step_disp(y1,ystep,r,p1,p2,n);
1086 /* if(result.y != 0.) */
1087 if (! APPROX(result.y, 0)) {
1088 return result;
1089 } else {
1090 return get_line_normal_disp(y1,y2,r,p1,p2,n);
1091 }
1092}
1093
1094
1095/*feed a point and a normal, and stats of a cylinder, it returns the displacement in the direction of the
1096 normal, or the vertical displacement(in case of stepping) that is needed for them not to intersect any more.*/
1097static struct point_XYZ get_point_disp(double y1, double y2, double ystep, double r, struct point_XYZ p1, struct point_XYZ n) {
1098 double y;
1099 struct point_XYZ result = {0,0,0};
1100 struct point_XYZ cp;
1101
1102 /*check if stepup.*/
1103 if((p1.y <= ystep && p1.y > y1 && p1.x*p1.x + p1.z*p1.z < r*r) && (n.y > STEPUP_MAXINCLINE) /*to steep to step on*/) {
1104 result.y = y1-p1.y;
1105 return result;
1106 }
1107
1108 /*select relevant cap*/
1109 y = (n.y < 0.) ? y2 : y1;
1110
1111 /*check if intersect cap*/
1112 /* if(n.y != 0) */
1113 if (! APPROX(n.y, 0)) {
1114 cp = project_on_yplane(p1,n,y);
1115 if(cp.x*cp.x + cp.z*cp.z < r*r) {
1116 VECDIFF(cp,p1,result);
1117 return result;
1118 }
1119 }
1120
1121 /*find intersections of point with cylinder side*/
1122 /* if(n.y != 1. && n.y != -1.) */ /*n.y == +/-1 makes n.x == n.z == 0, wich does div's by 0, besides making no sense at all. */
1123 if (! APPROX(n.y, 1) && ! APPROX(n.y, -1)) { /*n.y == +/-1 makes n.x == n.z == 0, wich does div's by 0, besides making no sense at all. */
1124 int nsect;
1125 double k1,k2;
1126 /*find pos of the point projected on surface of cylinder*/
1127 nsect = getk_intersect_line_with_ycylinder(&k1, &k2, r, p1, n);
1128 if(nsect != 0) {
1129 /*sect = p1 + k2 n*/
1130 if(k2 >= 0) return zero; /* wrong direction. we are out. */
1131 vecscale(&result,&n,k2);
1132 cp = result;
1133 VECADD(cp,p1);
1134
1135 if(cp.y > y1 && cp.y < y2) {
1136 return result;
1137 }
1138 }
1139
1140 }
1141
1142 return zero;
1143}
1144
1145
1146/*feed a box (a corner, and the three vertice sides) and the stats of a cylinder, it returns the
1147 displacement of the box that is needed for them not to intersect any more, with optionnal stepping displacement */
1148struct point_XYZ box_disp(double y1, double y2, double ystep, double r,struct point_XYZ p0, struct point_XYZ i, struct point_XYZ j, struct point_XYZ k) {
1149 struct point_XYZ p[8];
1150 struct point_XYZ n[6];
1151 struct point_XYZ maxdispv = {0,0,0};
1152 double maxdisp = 0;
1153 //struct point_XYZ middle;
1154 /*draw this up, you will understand: */
1155 static const int faces[6][4] = {
1156 {1,7,2,0},
1157 {2,6,3,0},
1158 {3,5,1,0},
1159 {5,3,6,4},
1160 {7,1,5,4},
1161 {6,2,7,4}
1162 };
1163 int ci;
1164
1165 for(ci = 0; ci < 8; ci++) p[ci] = p0;
1166
1167 /*compute points of box*/
1168 VECADD(p[1],i);
1169 VECADD(p[2],j);
1170 VECADD(p[3],k);
1171 VECADD(p[4],i); VECADD(p[4],j); VECADD(p[4],k); /*p[4]= i+j+k */
1172 VECADD(p[5],k); VECADD(p[5],i); /*p[6]= k+i */
1173 VECADD(p[6],j); VECADD(p[6],k); /*p[5]= j+k */
1174 VECADD(p[7],i); VECADD(p[7],j); /*p[7]= i+j */
1175
1176 /*compute normals, in case of perfectly orthogonal box, a shortcut exists*/
1177 veccross(&n[0],j,i);
1178 veccross(&n[1],k,j);
1179 veccross(&n[2],i,k);
1180 vecnormal(&n[0],&n[0]);
1181 vecnormal(&n[1],&n[1]);
1182 vecnormal(&n[2],&n[2]);
1183 vecscale(&n[3],&n[0],-1.);
1184 vecscale(&n[4],&n[1],-1.);
1185 vecscale(&n[5],&n[2],-1.);
1186
1187 /*what it says : middle of box */
1188 //middle = weighted_sum(p[0],p[4],.5);
1189
1190 for(ci = 0; ci < 6; ci++) {
1191 /*only clip faces "facing" origin */
1192 //if(vecdot(&n[ci],&middle) < 0.) {
1193 {
1194 struct point_XYZ pts[5];
1195 struct point_XYZ dispv;
1196 double disp;
1197 pts[0] = p[faces[ci][0]];
1198 pts[1] = p[faces[ci][1]];
1199 pts[2] = p[faces[ci][2]];
1200 pts[3] = p[faces[ci][3]];
1201 pts[4] = p[faces[ci][0]]; /* dug9 - for test split into 2 triangles for sphere test - no help with sphere*/
1202 //dispv = get_poly_disp(y1,y2,ystep,r,pts,4,n[ci]);
1203 dispv = get_poly_disp_2(pts,4,n[ci]);
1204 disp = vecdot(&dispv,&dispv);
1205
1206 /*keep result only if:
1207 displacement is positive
1208 displacement is smaller than minimum displacement up to date
1209 */
1210 if( (disp > FLOAT_TOLERANCE) && (disp > maxdisp) ){
1211 maxdisp = disp;
1212 maxdispv = dispv;
1213 }
1214 }
1215 }
1216 return maxdispv;
1217}
1218
1219/*
1220 * fast test to see if a box intersects a y-cylinder,
1221 * gives false positives.
1222 */
1223int fast_ycylinder_box_intersect(double y1, double y2, double r,struct point_XYZ pcenter, double xs, double ys, double zs) {
1224 double y = pcenter.y < 0 ? y1 : y2;
1225
1226 double lefteq = sqrt(y*y + r*r) + sqrt(xs*xs + ys*ys + zs*zs);
1227 return lefteq*lefteq > vecdot(&pcenter,&pcenter);
1228}
1229
1230
1231double *transformFULL4d(double *r4, double *a4, double *mat);
1232void __gluMultMatrixVecd(const GLDOUBLE matrix[16], const GLDOUBLE in[4], GLDOUBLE out[4]);
1233
1234int transformMBB4d(GLDOUBLE *rMBBmin, GLDOUBLE *rMBBmax, GLDOUBLE *matTransform, GLDOUBLE* inMBBmin, GLDOUBLE* inMBBmax, int isAffine)
1235{
1236 /* transform axes aligned minimum bounding box MBB via octo box / cuboid - will expand as necessary to cover original volume
1237 return value:
1238 1 - success
1239 0 - divide by 0, usually with projecting a point that's at 90 degrees to camera axis ie to the right, left, up, doown
1240 */
1241 struct point_XYZ abox[8];
1242 int i,j,k,m, iret;
1243 GLDOUBLE p[3],rx,ry,rz;
1244 iret = 1;
1245 /* generate an 8 corner box in shape space to represent the shape collision volume */
1246 m = 0;
1247 for(i=0;i<2;i++)
1248 {
1249 rx = i==0? inMBBmin[0] : inMBBmax[0];
1250 for(j=0;j<2;j++)
1251 {
1252 ry = j==0? inMBBmin[1] : inMBBmax[1];
1253 for(k=0;k<2;k++)
1254 {
1255 rz = k==0? inMBBmin[2] : inMBBmax[2];
1256 abox[m].x = rx;
1257 abox[m].y = ry;
1258 abox[m].z = rz;
1259 m++;
1260 }
1261 }
1262 }
1263
1264 /* transform the corners of the octo box */
1265 if(isAffine) {
1266 for(m=0;m<8;m++)
1267 transform(&abox[m],&abox[m],matTransform);
1268 }else{
1269 GLDOUBLE in[4];
1270 GLDOUBLE out[4];
1271 for(m=0;m<8;m++){
1272 pointxyz2double(in,&abox[m]);
1273 in[3]=1.0;
1274 __gluMultMatrixVecd(matTransform, in, out);
1275 //transformFULL4d(out,in, matTransform);
1276 if(0) if (fabs(out[3]) < .0001) {
1277 iret = 0;
1278 return iret;
1279 }
1280 vecscaled(out,out,1.0/out[3]);
1281 double2pointxyz(&abox[m],out);
1282 }
1283 }
1284 if(iret){
1285 /*find the MBB of the transformed octo box */
1286 //memcpy(rMBBmin,&abox[0],3*sizeof(GLDOUBLE)); //sizeof(struct point_XYZ));
1287 //memcpy(rMBBmax,&abox[0],3*sizeof(GLDOUBLE));
1288 pointxyz2double(rMBBmin,&abox[0]); //something to initialize them
1289 pointxyz2double(rMBBmax,&abox[0]);
1290 for(m=1;m<8;m++)
1291 {
1292 //memcpy(p,&abox[m],3*sizeof(GLDOUBLE));
1293 pointxyz2double(p,&abox[m]);
1294 for(i=0;i<3;i++)
1295 {
1296 rMBBmin[i] = DOUBLE_MIN(rMBBmin[i],p[i]);
1297 rMBBmax[i] = DOUBLE_MAX(rMBBmax[i],p[i]);
1298 }
1299 }
1300 }
1301 return iret;
1302}
1303void transformMBB(GLDOUBLE *rMBBmin, GLDOUBLE *rMBBmax, GLDOUBLE *matTransform, GLDOUBLE* inMBBmin, GLDOUBLE* inMBBmax){
1304 //AFFINE version, assumes matTransform is not projection / has no projection matrix
1305 int isAffine = 1, iret;
1306
1307 UNUSED(iret);
1308
1309 iret = transformMBB4d(rMBBmin, rMBBmax, matTransform, inMBBmin, inMBBmax,isAffine);
1310}
1311
1312
1313
1314
1315int fast_ycylinder_MBB_intersect_collisionSpace(double y1, double y2, double r, GLDOUBLE *shape2collision, GLDOUBLE *shapeMBBmin, GLDOUBLE *shapeMBBmax )
1316{
1317 /* goal: transform shape MBB to avatar/collision space, get its MBB there, and return 1 if the shape MBB intersects
1318 the avatar cylinderical collision volume MBB
1319 Issue: you'll likely have more false positives with the avatar-space MBB intersection test (versus shape-space)
1320 shapes in shape space are statistically more often cuboid and axes-aligned - think of wall-like objects.
1321 As a result a MBB in shape space is relatively smaller than the MBB recomputed after transforming to
1322 avatar space. (Same could be said in theory for the avatar cylinder going the other way, but when I think of wall
1323 objects I'd rather be doing it in shape space.
1324 */
1325 int i;
1326 GLDOUBLE smin[3], smax[3], avmin[3], avmax[3];
1327
1328 /* generate mins and maxes for avatar cylinder in avatar space to represent the avatar collision volume */
1329 for(i=0;i<3;i++)
1330 {
1331 avmin[i] = -r;
1332 avmax[i] = r;
1333 }
1334 avmin[1] = y1; avmax[1] = y2;
1335
1336 transformMBB(smin,smax,shape2collision,shapeMBBmin,shapeMBBmax);
1337 return overlapMBBs(avmin,avmax,smin,smax);
1338}
1339
1340
1341int fast_sphere_MBB_intersect_collisionSpace(double r, GLDOUBLE *shape2collision, GLDOUBLE *shapeMBBmin, GLDOUBLE *shapeMBBmax )
1342{
1343 /* goal: transform shape MBB to avatar/collision space, get its MBB there, and return 1 if the shape MBB intersects
1344 the avatar spherical collision volume MBB
1345 */
1346 int i;
1347 GLDOUBLE smin[3], smax[3], avmin[3], avmax[3];
1348
1349 /* generate mins and maxes for avatar cylinder in avatar space to represent the avatar collision volume */
1350 for(i=0;i<3;i++)
1351 {
1352 avmin[i] = -r;
1353 avmax[i] = r;
1354 }
1355 transformMBB(smin,smax,shape2collision,shapeMBBmin,shapeMBBmax);
1356 return overlapMBBs(avmin,avmax,smin,smax);
1357}
1358
1359
1360/*fast test to see if a cone intersects a y-cylinder. */
1361/*gives false positives. */
1362int fast_ycylinder_cone_intersect(double y1, double y2, double r,struct point_XYZ pcenter, double halfheight, double baseradius) {
1363 double y = pcenter.y < 0 ? y1 : y2;
1364
1365 double lefteq = sqrt(y*y + r*r) + sqrt(halfheight*halfheight + baseradius*baseradius);
1366 return lefteq*lefteq > vecdot(&pcenter,&pcenter);
1367}
1368
1369
1370
1371/*algorithm is approximative */
1372/*basically, it does collision with a triangle on a plane that passes through the origin.*/
1373struct point_XYZ cone_disp(double y1, double y2, double ystep, double r, struct point_XYZ base, struct point_XYZ top, double baseradius) {
1374
1375 struct point_XYZ i; /* cone axis vector*/
1376 double h; /* height of cone*/
1377 struct point_XYZ tmp;
1378 struct point_XYZ bn; /* direction from cone to cylinder*/
1379 struct point_XYZ side; /* side of base in direction of origin*/
1380 struct point_XYZ normalbase; /* collision normal of base (points downwards)*/
1381 struct point_XYZ normalside; /* collision normal of side (points outside)*/
1382 struct point_XYZ normaltop; /* collision normal of top (points up)*/
1383 //struct point_XYZ bn_normal; /* bn, normalized;*/
1384 struct point_XYZ mindispv= {0,0,0};
1385 double mindisp = 1E99;
1386
1387 /*find closest point of cone base to origin. */
1388
1389 vecscale(&bn,&base,-1.0);
1390
1391 VECDIFF(top,base,i);
1392 vecscale(&tmp,&i,- vecdot(&i,&bn)/vecdot(&i,&i));
1393 VECADD(bn,tmp);
1394 /* if(vecnormal(&bn,&bn) == 0.) */
1395 if (APPROX(vecnormal(&bn,&bn), 0)) {
1396 /* origin is aligned with cone axis */
1397 /* must use different algorithm to find side */
1398 struct point_XYZ tmpn = i;
1399 struct point_XYZ tmpj;
1400 vecnormal(&tmpn,&tmpn);
1401 make_orthogonal_vector_space(&bn,&tmpj,tmpn);
1402 //bn_normal = bn;
1403 }
1404 vecscale(&side,&bn,baseradius);
1405 VECADD(side,base);
1406
1407 /* find normals ;*/
1408 h = vecnormal(&i,&i);
1409 normaltop = i;
1410 vecscale(&normalbase,&normaltop,-1.0);
1411 vecscale(&i,&i,-baseradius);
1412 vecscale(&normalside,&bn,-h);
1413 VECADD(normalside,i);
1414 vecnormal(&normalside,&normalside);
1415 vecscale(&normalside,&normalside,-1.0);
1416
1417 {
1418 /*get minimal displacement*/
1419 struct point_XYZ dispv;
1420 double disp;
1421
1422 if( vecdot(&normalside,&top) < 0. ) {
1423 dispv = get_line_disp(y1,y2,ystep,r,top,side,normalside);
1424 disp = vecdot(&dispv,&dispv);
1425 if(disp < mindisp)
1426 mindispv = dispv, mindisp = disp;
1427 }
1428
1429 if( vecdot(&normalbase,&base) < 0. ) {
1430 dispv = get_line_disp(y1,y2,ystep,r,base,side,normalbase);
1431 disp = vecdot(&dispv,&dispv);
1432 if(disp < mindisp)
1433 mindispv = dispv, mindisp = disp;
1434 }
1435
1436 if( vecdot(&normaltop,&top) < 0. ) {
1437 dispv = get_point_disp(y1,y2,ystep,r,top,normaltop);
1438 disp = vecdot(&dispv,&dispv);
1439 /*i don't like "disp !=0." there should be a different condition for
1440 * non applicability.*/
1441 /* if(disp != 0. && disp < mindisp) */
1442 if(! APPROX(disp, 0) && disp < mindisp)
1443 mindispv = dispv, mindisp = disp;
1444 }
1445 }
1446
1447 return mindispv;
1448}
1449
1450
1451/*algorithm is approximative */
1452/*basically, it does collision with a rectangle on a plane that passes through the origin.*/
1453struct point_XYZ cylinder_disp(double y1, double y2, double ystep, double r, struct point_XYZ base, struct point_XYZ top, double baseradius) {
1454
1455 struct point_XYZ i; /* cone axis vector*/
1456 //double h; /* height of cone*/
1457 struct point_XYZ tmp;
1458 struct point_XYZ bn; /* direction from cone to cylinder*/
1459 struct point_XYZ sidetop; /* side of top in direction of origin*/
1460 struct point_XYZ sidebase; /* side of base in direction of origin*/
1461 struct point_XYZ normalbase; /* collision normal of base (points downwards)*/
1462 struct point_XYZ normalside; /* collision normal of side (points outside)*/
1463 struct point_XYZ normaltop; /* collision normal of top (points upwards)*/
1464 struct point_XYZ mindispv= {0,0,0};
1465 double mindisp = 1E99;
1466
1467 /*find closest point of cone base to origin. */
1468
1469 vecscale(&bn,&base,-1.0);
1470
1471 VECDIFF(top,base,i);
1472 vecscale(&tmp,&i,- vecdot(&i,&bn)/vecdot(&i,&i));
1473 VECADD(bn,tmp);
1474 /* if(vecnormal(&bn,&bn) == 0.) */
1475 if (APPROX(vecnormal(&bn,&bn), 0)) {
1476 /* origin is aligned with cone axis */
1477 /* must use different algorithm to find side */
1478 struct point_XYZ tmpn = i;
1479 struct point_XYZ tmpj;
1480 vecnormal(&tmpn,&tmpn);
1481 make_orthogonal_vector_space(&bn,&tmpj,tmpn);
1482 }
1483 vecscale(&sidebase,&bn,baseradius);
1484 sidetop = top;
1485 VECADD(sidetop,sidebase);
1486 VECADD(sidebase,base);
1487
1488 /* find normals ;*/
1489 //h = vecnormal(&i,&i);
1490 normaltop = i;
1491 vecscale(&normalbase,&normaltop,-1.0);
1492 normalside = bn;
1493
1494 {
1495 /*get minimal displacement*/
1496 struct point_XYZ dispv;
1497 double disp;
1498
1499 if( vecdot(&normalside,&sidetop) < 0. ) {
1500 dispv = get_line_disp(y1,y2,ystep,r,sidetop,sidebase,normalside);
1501 disp = vecdot(&dispv,&dispv);
1502 if(disp < mindisp)
1503 mindispv = dispv, mindisp = disp;
1504 }
1505
1506 if( vecdot(&normalbase,&base) < 0. ) {
1507 dispv = get_line_disp(y1,y2,ystep,r,base,sidebase,normalbase);
1508 disp = vecdot(&dispv,&dispv);
1509 if(disp < mindisp)
1510 mindispv = dispv, mindisp = disp;
1511 }
1512
1513 if( vecdot(&normaltop,&top) < 0. ) {
1514 dispv = get_line_disp(y1,y2,ystep,r,top,sidetop,normaltop);
1515 disp = vecdot(&dispv,&dispv);
1516 if( disp < mindisp)
1517 mindispv = dispv, mindisp = disp;
1518 }
1519 }
1520
1521 return mindispv;
1522}
1523
1524
1525
1526static int intersectionHeightOfVerticalLineWithSurfaceElement(double* height, struct point_XYZ* p, int num, struct point_XYZ* n, double *tmin, double *tmax )
1527{
1528
1529 /* Intersects a Y vertical infinite line passing through origin with a convex polygon
1530 convex polygon - like a triangle or quad or pentagon. Not like a star or general polygon like font glyph outline
1531 etc which could be concave like a U
1532 Input:
1533 p[num] - polygon vertices
1534 n - polygon normal
1535 returns:
1536 0 if intersection fails due to either vertical polygon face or intesection point outside polygon
1537 height - if inside, this will be the Y height relative to the origin of the intersection point
1538 */
1539 /* step 1 compute the infinite plane through the points p. Use the normal N passed in. */
1540 int overlap;
1541 struct point_XYZ D;
1542 double dd, ndotD;
1543 overlap = tmax[0] >= 0.0 && tmin[0] <= 0.0 && tmax[2] >= 0.0 && tmin[2] <= 0.0;
1544 if(!overlap) return 0;
1545 /* end cheap MBR test */
1546 dd = -vecdot(&p[0],n);
1547 /* the Origin is 0,0,0 and N*O is 0 */
1548 /* D is the ray direction - our zenith vector */
1549 D.x = 0.0;
1550 D.y = 1.0;
1551 D.z = 0.0;
1552 ndotD = vecdot(n,&D);
1553 /* slope check - if near vertical should we skip it?. */
1554 if( fabs(ndotD) < .1 )
1555 {
1556 return 0; /* vertical wall */
1557 }
1558 *height = - dd/ndotD;
1559 /* step 2 determine if inside the triangle */
1560 /* in theory if the cross products (D*height - p[i]) x (p[i+1] - p[i])
1561 point in the same general direction, it's inside (if one alternates, its outside)*/
1562 D.y = *height;
1563 return pointOnPlaneInsidePoly(D,p,num,n);
1564}
1565
1566void accumulateFallingClimbing(double y1, double y2, double ystep, struct point_XYZ *p, int num, struct point_XYZ nused, double *tmin, double *tmax)
1567{
1568 struct sFallInfo *fi = FallInfo();
1569
1570 if(fi->walking)
1571 {
1572 /* Goal: Falling - if we're floating above the terrain we'll have no regular collision
1573 so we need special test to see if we should fall. If climbing we nullify any fall.
1574 */
1575 double hh;
1576 if( intersectionHeightOfVerticalLineWithSurfaceElement(&hh,&p[0],num,&nused, tmin, tmax))
1577 {
1578 /* terrain below avatar at 0,0,0? */
1579 double hhbelowy1 = hh - y1;
1580 if( hh < 0.0 )
1581 {
1582 /* falling */
1583 if( hh < y1 && hh > -fi->fallHeight)
1584 {
1585 /* FALLING */
1586 if(fi->hits ==0)
1587 fi->hfall = hhbelowy1; //hh - y1;
1588 else
1589 if(hhbelowy1 > fi->hfall) fi->hfall = hhbelowy1; //hh - y1;
1590 fi->hits++;
1591 fi->isFall = 1;
1592 }else
1593 /* regular below / nadir collision - below avatar center but above avatar's feet which are at 0.0 - avatar.height*/
1594 if( hh >= y1 ) /* && hh <= (y1-ystep) ) //no step height implementation */
1595 {
1596 /* CLIMBING. handled elsewhere for displacements, except annihilates any fall*/
1597 fi->canFall = 0;
1598
1599 if( fi->isClimb == 0 )
1600 fi->hclimb = hhbelowy1; //hh - y1;
1601 else
1602 fi->hclimb = DOUBLE_MAX(fi->hclimb,hhbelowy1);
1603 fi->isClimb = 1;
1604 }
1605 /* no stepheight implementation here
1606 else
1607 if(hh > (y1-ystep) && hh < 0.0 )
1608 {
1609 // blocked by a high step, no climb
1610 printf("S");
1611 }
1612 */
1613
1614
1615 }else{
1616 /* regular above / zenith collision */
1617 if( hh > 0.0 && hh < y2 )
1618 {
1619 /* HEAD-BUMP handled elsewhere */
1620 if(0) printf("c");
1621 }
1622 else
1623 if(0) printf("B"); /* head is CLEAR of ceiling point */
1624 }
1625 }
1626 }
1627}
1628
1629/*used by polyrep_disp
1630 - coming in - all coords are already transformed into collision space
1631 - sets up avatar collision volume: a cyclinder for walking/stepping, a sphere for fly/examine
1632 - tests triangle by triangle for a collision
1633 - accumulates displacements from collisions
1634y1,y2,ystep,r (usually abottom,atop,astep,width from naviinfo height,step,width for the avatar) are in global scale coordinates
1635 pr.actualCoord[pr->ntri]
1636 n[pr->ntri] - one normal for each triangle
1637dispsum.xyz - output - sum of collision displacement vectors - a mean will be computed by caller
1638flags - doublesided, front/back facing hints, no-stepping (?)
1639*/
1640//struct point_XYZ polyrep_disp_rec(double y1, double y2, double ystep, double r, struct X3D_PolyRep* pr, struct point_XYZ* n, struct point_XYZ dispsum, prflags flags) {
1641static struct point_XYZ polyrep_disp_rec2(struct X3D_PolyRep* pr, struct point_XYZ* n, struct point_XYZ dispsum, prflags flags) {
1642 struct point_XYZ p[3];
1643 double maxdisp = 0;
1644 struct point_XYZ maxdispv = {0,0,0};
1645 double disp;
1646 struct point_XYZ dispv;
1647 int i;
1648 int frontfacing;
1649 int ccw;
1650
1651 ccw = pr->ccw;
1652
1653//printf ("start polyrep_disp_rec2\n");
1654
1655 for(i = 0; i < pr->ntri; i++)
1656 {
1657 p[0].x = pr->actualCoord[pr->cindex[i*3]*3] +dispsum.x;
1658 p[0].y = pr->actualCoord[pr->cindex[i*3]*3+1] +dispsum.y;
1659 p[0].z = pr->actualCoord[pr->cindex[i*3]*3+2] +dispsum.z;
1660
1661 if (ccw) frontfacing = (vecdot(&n[i],&p[0]) < 0); /*if normal facing avatar. avatar is at 0,0,0. If vector P going opposite direction to N, P*N will be negative */
1662 else frontfacing = (vecdot(&n[i],&p[0]) >= 0); /*if ccw facing avatar */
1663
1664 /* printf ("polyrep_disp_rec, frontfacing %d BACKFACING %d FRONTFACING %d DOUBLESIDED %d\n",
1665 frontfacing, flags & PR_BACKFACING, flags & PR_FRONTFACING, flags & PR_DOUBLESIDED); */
1666 /* use if either:
1667 -frontfacing and not in doubleside mode;
1668 -if in doubleside mode:
1669 use if either:
1670 -PR_FRONTFACING or PR_BACKFACING not yet specified
1671 -fontfacing and PR_FRONTFACING specified
1672 -not frontfacing and PR_BACKFACING specified
1673 */
1674 if( (frontfacing && !(flags & PR_DOUBLESIDED) )
1675 || ( (flags & PR_DOUBLESIDED) && !(flags & (PR_FRONTFACING | PR_BACKFACING) ) )
1676 || (frontfacing && (flags & PR_FRONTFACING))
1677 || (!frontfacing && (flags & PR_BACKFACING)) )
1678 {
1679
1680 struct point_XYZ nused;
1681 p[1].x = pr->actualCoord[pr->cindex[i*3+1]*3] +dispsum.x;
1682 p[1].y = pr->actualCoord[pr->cindex[i*3+1]*3+1] +dispsum.y;
1683 p[1].z = pr->actualCoord[pr->cindex[i*3+1]*3+2] +dispsum.z;
1684 p[2].x = pr->actualCoord[pr->cindex[i*3+2]*3] +dispsum.x;
1685 p[2].y = pr->actualCoord[pr->cindex[i*3+2]*3+1] +dispsum.y;
1686 p[2].z = pr->actualCoord[pr->cindex[i*3+2]*3+2] +dispsum.z;
1687
1688 if(frontfacing)
1689 {
1690 nused = n[i];
1691 } else { /*can only be here in DoubleSided mode*/
1692 /*reverse polygon orientation, and do calculations*/
1693 vecscale(&nused,&n[i],-1.0);
1694 }
1695 /* ready to start testing the triangle against our collision volume (sphere/cylinder/line) tests.
1696 The avatar collision volume -
1697 A.for flying/examining:
1698 1. sphere.
1699
1700 B.For walking:
1701 1. sphere (head) <<<<< problem I do not like this displacement logic when walking
1702 because you'll end up floating over something that's just below avatar 0,0,0
1703 which for walking is wrong - you should collide with the cylinder going from top to bottom.
1704 Recommendation: either always do both sphere and cylinder or
1705 just do #2 cylinder abottom to atop for head and body.
1706 2. cylinder (body)
1707 3. line - climb/fall (legs,feet)
1708 Order of tests (so can exit early to save unnecessary computations):
1709 sphere > cylinder > climb > fall
1710 if you have a sphere hit you don't need to test the rest.
1711 If you have a cylinder hit, you don't need to test climb/fall.
1712 if you have a climb, you can skip the fall.
1713 otherwise test for fall.
1714
1715 fast tests using MBB in the caller elliminate some volumes
1716 docollision - sphere and cyclinder should be tested
1717 dofall - climb and fall should be tested
1718
1719 Total logic for walking:
1720 collision = docollision
1721 if(docollision)
1722 do sphere
1723 if not sphere
1724 do cylinder
1725 if not cylinder
1726 collision = 0
1727 if( dofall && !collision )
1728 do line
1729 if not climb
1730 do fall
1731 */
1732 // printf ("calling get_poly_disp_2 at %s:%d\n",__FILE__,__LINE__);
1733
1734 dispv = get_poly_disp_2(p, 3, nused); //get_poly_disp_2(y1,y2,ystep,r, p, 3, nused);
1735 disp = vecdot(&dispv,&dispv);
1736
1737
1738 #ifdef DEBUGPTS
1739 if(dispv.x != 0 || dispv.y != 0 || dispv.z != 0)
1740 printf("polyd: (%f,%f,%f) |%f|\n",dispv.x,dispv.y,dispv.z,disp);
1741 #endif
1742
1743
1744 /*keep result only if:
1745 displacement is positive
1746 displacement is smaller than minimum displacement up to date
1747 */
1748 if( (disp > FLOAT_TOLERANCE) && (disp > maxdisp) )
1749 {
1750 maxdisp = disp;
1751 maxdispv = dispv;
1752 }
1753 }
1754 }
1755
1756 VECADD(dispsum,maxdispv);
1757 //printf ("end polyrep_disp_rec2, dispsum %lf %lf %lf at end\n",dispsum.x,dispsum.y,dispsum.z);
1758 return dispsum;
1759}
1760
1761#undef POLYREP_DISP2_PERFORMANCE
1762#ifdef POLYREP_DISP2_PERFORMANCE
1763
1764static bool timing = FALSE;
1765static double startTime = 0.0;
1766static double stopTime = 0.0;
1767static double accumTime = 0.0;
1768static int counter = 0;
1769
1770#endif
1771
1772/*uses sphere displacement, and a cylinder for stepping
1773 y1, y2, ystep, r - (usually abottom, atop, astep, awidth) are from naviiinfo avatar height, step, width
1774 pr - will be transformed by mat from raw shape coordinates into collision space:
1775 Fly/Examine: avatar space
1776 Walk: Bound-Viewpoint-Vertical-aligned Avatar-centric BVVA space.
1777 flags -
1778*/
1779struct point_XYZ polyrep_disp2(struct X3D_PolyRep pr, GLDOUBLE* mat, prflags flags) {
1780 int i;
1781 unsigned int maxc;
1782 ppcollision pp = (ppcollision)gglobal()->collision.prv;
1783
1784#ifdef POLYREP_DISP2_PERFORMANCE
1785 if (!timing) {
1786 printf ("start timing polyrep_disp2\n");
1787 timing = TRUE;
1788
1789 }
1790 startTime = Time1970sec();
1791#endif
1792
1793
1794#ifdef HAVE_OPENCL
1795
1796 if ((pr.VBO_buffers[VERTEX_VBO] != 0) && pp->OpenCL_Collision_Program_initialized) {
1797 ttglobal tg = gglobal();
1798 float awidth = (float) tg->Bindable.naviinfo.width; /*avatar width*/
1799
1800 float mymat[16];
1801 for (i=0; i<16; i++) {
1802 mymat[i] = (float) mat[i];
1803 }
1804
1805 pp->res = run_non_walk_collide_program(
1806 pr.VBO_buffers[VERTEX_VBO],pr.VBO_buffers[INDEX_VBO],mymat, pr.ntri,
1807 pr.ccw, flags, awidth);
1808
1809 //printf ("openCL sez: move us %f %f %f\n",pp->res.x,pp->res.y,pp->res.z);
1810
1811
1812#ifdef POLYREP_DISP2_PERFORMANCE
1813 stopTime = Time1970sec();
1814 accumTime += stopTime - startTime;
1815
1816 if (counter == 25) {
1817 printf ("polyrep_disp2, averaged over 25 runs: %f\n",
1818 accumTime/25.0);
1819 counter = 0;
1820 accumTime = 0.0;
1821 }
1822
1823 counter ++;
1824#endif
1825
1826 return pp->res;
1827 }
1828
1829 /* if we are here, there was an OpenCL issue and we have to do this by software */
1830#endif
1831
1832
1833 pp->res.x=0.0; pp->res.y=0.0; pp->res.z=0.0;
1834 maxc = 0; /* highest cindex, used to point into prd_newc_floats structure.*/
1835
1836 for(i = 0; i < pr.ntri*3; i++) {
1837 if (pr.cindex[i] > maxc) {maxc = pr.cindex[i];}
1838 }
1839
1840 /*transform all points from raw shape to viewer(fly) or BVAAC(walk) space */
1841 if (maxc> pp->prd_newc_floats_size) {
1842 pp->prd_newc_floats = REALLOC(pp->prd_newc_floats,maxc*9*sizeof(float));
1843 pp->prd_newc_floats_size = maxc;
1844 }
1845
1846
1847 for(i = 0; i < pr.ntri*3; i++) {
1848 transformf(&pp->prd_newc_floats[pr.cindex[i]*3],&pr.actualCoord[pr.cindex[i]*3],mat);
1849 }
1850
1851 pr.actualCoord = pp->prd_newc_floats; /*remember, coords are only replaced in our local copy of PolyRep */
1852
1853
1854 /*pre-calculate face normals */
1855 if (pr.ntri> pp->prd_normals_size) {
1856 pp->prd_normals = REALLOC(pp->prd_normals,pr.ntri*sizeof(struct point_XYZ));
1857 pp->prd_normals_size = pr.ntri;
1858 }
1859
1860 for(i = 0; i < pr.ntri; i++) {
1861 polynormalf(&pp->prd_normals[i],&pr.actualCoord[pr.cindex[i*3]*3],
1862 &pr.actualCoord[pr.cindex[i*3+1]*3],&pr.actualCoord[pr.cindex[i*3+2]*3]);
1863 }
1864
1865 pp->res = polyrep_disp_rec2(&pr,pp->prd_normals,pp->res,flags); //polyrep_disp_rec(y1,y2,ystep,r,&pr,prd_normals,res,flags);
1866
1867 /* printf ("polyrep_disp_rec2 tells us to move: %f %f %f\n",pp->res.x, pp->res.y, pp->res.z); */
1868
1869 pr.actualCoord = 0;
1870
1871#ifdef POLYREP_DISP2_PERFORMANCE
1872 stopTime = Time1970sec();
1873 accumTime += stopTime - startTime;
1874
1875 if (counter == 25) {
1876 printf ("polyrep_disp2, averaged over 25 runs: %f\n",
1877 accumTime/25.0);
1878 counter = 0;
1879 accumTime = 0.0;
1880 }
1881
1882 counter ++;
1883#endif
1884
1885 return pp->res;
1886}
1887
1888
1889
1890
1891/*Optimized polyrep_disp for planar polyreps.
1892 Used for text.
1893 planar_polyrep_disp computes the normal using the first polygon, if no normal is specified (if it is zero).
1894 JAS - Normal is always specified now. (see VRMLRend.pm for invocation)
1895*/
1896static struct point_XYZ planar_polyrep_disp_rec(double y1, double y2, double ystep, double r, struct X3D_PolyRep* pr, struct point_XYZ n, struct point_XYZ dispsum, prflags flags) {
1897 struct point_XYZ p[3];
1898 double lmaxdisp = 0;
1899 struct point_XYZ maxdispv = {0,0,0};
1900 double disp;
1901 struct point_XYZ dispv;
1902 /* static int recursion_count = 0; */
1903 int i;
1904 int frontfacing;
1905 ppcollision pp = (ppcollision)gglobal()->collision.prv;
1906
1907 p[0].x = pr->actualCoord[pr->cindex[0]*3] +dispsum.x;
1908 p[0].y = pr->actualCoord[pr->cindex[0]*3+1] +dispsum.y;
1909 p[0].z = pr->actualCoord[pr->cindex[0]*3+2] +dispsum.z;
1910
1911 frontfacing = (vecdot(&n,&p[0]) < 0); /*if normal facing avatar */
1912
1913 if(!frontfacing && !(flags & PR_DOUBLESIDED)) return dispsum;
1914
1915 if(!frontfacing) vecscale(&n,&n,-1.0);
1916
1917 for(i = 0; i < pr->ntri; i++) {
1918 p[0].x = pr->actualCoord[pr->cindex[i*3]*3] +dispsum.x;
1919 p[0].y = pr->actualCoord[pr->cindex[i*3]*3+1] +dispsum.y;
1920 p[0].z = pr->actualCoord[pr->cindex[i*3]*3+2] +dispsum.z;
1921 p[1].x = pr->actualCoord[pr->cindex[i*3+1]*3] +dispsum.x;
1922 p[1].y = pr->actualCoord[pr->cindex[i*3+1]*3+1] +dispsum.y;
1923 p[1].z = pr->actualCoord[pr->cindex[i*3+1]*3+2] +dispsum.z;
1924 p[2].x = pr->actualCoord[pr->cindex[i*3+2]*3] +dispsum.x;
1925 p[2].y = pr->actualCoord[pr->cindex[i*3+2]*3+1] +dispsum.y;
1926 p[2].z = pr->actualCoord[pr->cindex[i*3+2]*3+2] +dispsum.z;
1927
1928 //dispv = get_poly_disp(y1,y2,ystep, r, p, 3, n);
1929 dispv = get_poly_disp_2(p, 3, n);
1930 disp = -pp->get_poly_mindisp; /*global variable. was calculated inside poly_normal_disp already. */
1931
1932#ifdef DEBUGPTS
1933 printf("polyd: (%f,%f,%f) |%f|\n",dispv.x,dispv.y,dispv.z,disp);
1934#endif
1935
1936 /*keep result only if:
1937 displacement is positive
1938 displacement is bigger than maximum displacement up to date
1939 */
1940 if((disp > FLOAT_TOLERANCE) && (disp > lmaxdisp)) {
1941 lmaxdisp = disp;
1942 maxdispv = dispv;
1943 }
1944
1945 }
1946 VECADD(dispsum,maxdispv);
1947 return dispsum;
1948
1949}
1950
1951
1952struct point_XYZ planar_polyrep_disp(double y1, double y2, double ystep, double r, struct X3D_PolyRep pr, GLDOUBLE* mat, prflags flags, struct point_XYZ n) {
1953 int i;
1954 unsigned int maxc;
1955 ppcollision pp = (ppcollision)gglobal()->collision.prv;
1956
1957
1958 pp->res.x=0.0; pp->res.y=0.0; pp->res.z=0.0;
1959 maxc = 0; /* highest cindex, used to point into newc structure.*/
1960
1961 for(i = 0; i < pr.ntri*3; i++) {
1962 if (pr.cindex[i] > maxc) {maxc = pr.cindex[i];}
1963 }
1964
1965 /*transform all points to viewer space */
1966 if (maxc> pp->prd_newc_floats_size) {
1967 pp->prd_newc_floats = REALLOC(pp->prd_newc_floats,maxc*9*sizeof(float));
1968 pp->prd_newc_floats_size = maxc;
1969 }
1970
1971 for(i = 0; i < pr.ntri*3; i++) {
1972 transformf(&pp->prd_newc_floats[pr.cindex[i]*3],&pr.actualCoord[pr.cindex[i]*3],mat);
1973 }
1974 pr.actualCoord = pp->prd_newc_floats; /*remember, coords are only replaced in our local copy of PolyRep */
1975
1976 /*if normal not speced, calculate it */
1977 /* if(n.x == 0 && n.y == 0 && n.z == 0.) */
1978 if(APPROX(n.x, 0) && APPROX(n.y, 0) && APPROX(n.z, 0)) {
1979 polynormalf(&n,&pr.actualCoord[pr.cindex[0]*3],&pr.actualCoord[pr.cindex[1]*3],&pr.actualCoord[pr.cindex[2]*3]);
1980 }
1981
1982 pp->res = planar_polyrep_disp_rec(y1,y2,ystep,r,&pr,n,pp->res,flags);
1983
1984 return pp->res;
1985}
1986
1987
1988
1989
1990
1991
1992static void get_collisionoffset(double *x, double *y, double *z)
1993{
1994 struct sCollisionInfo *ci;
1995 struct sFallInfo *fi;
1996 struct sNaviInfo *naviinfo;
1997 struct point_XYZ xyz;
1998 struct point_XYZ res;
1999 ttglobal tg = gglobal();
2000 ci = CollisionInfo();
2001 fi = FallInfo();
2002 naviinfo = (struct sNaviInfo*)tg->Bindable.naviinfo;
2003 res = ci->Offset;
2004 /* collision.offset should be in collision space coordinates: fly/examine: avatar space, walk: BVVA space */
2005 /* uses mean direction, with maximum distance */
2006
2007 /* xyz is in collision space- fly/examine: avatar space, walk: BVVA space */
2008 xyz.x = xyz.y = xyz.z = 0.0;
2009
2010 if(ci->Count > 0 && !APPROX(vecnormal(&res, &res),0.0) )
2011 vecscale(&xyz, &res, sqrt(ci->Maximum2));
2012
2013 /* for WALK + collision */
2014 if(fi->walking)
2015 {
2016 if(fi->canFall && fi->isFall )
2017 {
2018 /* canFall == true if we aren't climbing, isFall == true if there's no climb, and there's geom to fall to */
2019 double floatfactor = .1;
2020 if(fi->allowClimbing) floatfactor = 0.0; /*popcycle method */
2021 if(fi->smoothStep){
2022 //its socially acceptable to float a bit when falling...
2023 double fallstep = DOUBLE_MIN(fi->hfall,fi->hfall * 2.0 * (TickTime() - lastTime()));
2024 //if(0) xyz.y = DOUBLE_MAX(fi->hfall,-fi->fallStep) + naviinfo->height*floatfactor; //pre-2018 method
2025 xyz.y = DOUBLE_MAX(fi->hfall,fallstep); //+ naviinfo->height*floatfactor;
2026 }else{
2027 xyz.y = fi->hfall + naviinfo->height*floatfactor; //.1;
2028 }
2029 }
2030 if(fi->isClimb && fi->allowClimbing)
2031 {
2032 /* stepping up normally handled by cyclindrical collision, but there are settings to use this climb instead */
2033 //but when climbing its not cool to dig underneath the terrain, so assymmetrical, a bit faster climbing
2034 //than falling.
2035 if(fi->smoothStep && FALSE){
2036 double fallstep = DOUBLE_MIN(fi->hclimb, fi->hclimb * 8.0 * (TickTime() - lastTime()));
2037 //if(0) xyz.y = DOUBLE_MIN(fi->hclimb,fi->fallStep); //pre-2018 method
2038 xyz.y = DOUBLE_MIN(fi->hclimb,fallstep);
2039 }else{
2040 xyz.y = fi->hclimb;
2041 }
2042 }
2043 if(fi->isPenetrate)
2044 {
2045 /*over-ride everything else*/
2046 xyz = fi->pencorrection;
2047 }
2048 }
2049 /* now convert collision-space deltas to avatar space via collision2avatar- fly/examine: identity (do nothing), walk:BVVA2A */
2050 transform3x3(&xyz,&xyz,fi->collision2avatar);
2051 /* now xyz is in avatar space, ready to be added to avatar viewer.pos */
2052 *x = xyz.x;
2053 *y = xyz.y;
2054 *z = xyz.z;
2055 /* another transform possible: from avatar space into navigation space. fly/examine: identity walk: A2BVVA*/
2056}
2057struct point_XYZ viewer_lastP_get();
2058void render_collisions(int Viewer_type) {
2059 struct point_XYZ v;
2060 struct sCollisionInfo *ci;
2061 struct sFallInfo *fi;
2062 struct sNaviInfo *naviinfo;
2063 naviinfo = (struct sNaviInfo*)gglobal()->Bindable.naviinfo;
2064
2065 if(!(Viewer_type == VIEWER_WALK || Viewer_type == VIEWER_FLY)) return; //no collisions
2066 ci = CollisionInfo();
2067 fi = FallInfo();
2068 ci->Offset.x = 0;
2069 ci->Offset.y = 0;
2070 ci->Offset.z = 0;
2071 ci->Count = 0;
2072 ci->Maximum2 = 0.;
2073
2074 /* popcycle shaped avatar collision volume: ground to stepheight is a ray, stepheight to avatarheight is a cylinder,
2075 and a sphere on top?
2076 -keeps cyclinder from dragging in terrain mesh, easy to harmonize fall and climb math so not fighting (now a ray both ways)
2077 -2 implementations: analytical cyclinder, sampler
2078 The sampler method intersects line segments radiating from the the avatar axis with shape facets - misses small shapes but good
2079 for walls and floors; intersection math is simple: line intersect plane.
2080 */
2081 fi->fallHeight = 100.0*naviinfo->height; //200.0; /* when deciding to fall, how far down do you look for a landing surface before giving up and floating */
2082 fi->climbHeight = 100.0*naviinfo->height; // 200.0; //sometimes you get underneath the terrain. At what point should we cimb you out automatically
2083 fi->fallStep = 1.0; /* maximum height to fall on one frame */
2084 fi->walking = Viewer_type == VIEWER_WALK; //viewer_type == VIEWER_WALK;
2085 fi->canFall = fi->walking; /* && COLLISION (but we wouldn't be in here if not). Will be set to 0 if a climb is found. */
2086 fi->hits = 0; /* counter (number of vertical hits) set to zero here once per frame */
2087 fi->isFall = 0; /*initialize here once per frame - flags if there's a fall found */
2088 fi->isClimb = 0; /* initialize here each frame */
2089 fi->smoothStep = 1; /* [1] setting - will only fall by fallstep on a frame rather than the full hfall */
2090 fi->allowClimbing = 1; /* [0] - setting - 0=climbing done by collision with cyclinder 1=signals popcycle avatar collision volume and allows single-footpoint climbing */
2091
2092 fi->canPenetrate = 1; /* setting - 0= don't check for wall penetration 1= check for wall penetration */
2093 fi->isPenetrate = 0; /* set to zero once per loop and will come back 1 if there was a penetration detected and corrected */
2094
2095 /* at this point we know the navigation mode and the pose of the avatar, and of the boundviewpoint
2096 so pre-compute some handy matrices for collision calcs - the avatar2collision and back (a tilt matrix for gravity direction)
2097 */
2098 if(fi->walking)
2099 {
2100 /*bound viewpoint vertical aligned gravity as per specs*/
2101 avatar2BoundViewpointVerticalAvatar(fi->avatar2collision, fi->collision2avatar);
2102 }
2103 else
2104 {
2105 /* when flying or examining, no gravity - up is your avatar's up */
2106 loadIdentityMatrix(fi->avatar2collision);
2107 loadIdentityMatrix(fi->collision2avatar);
2108 }
2109
2110 /* wall penetration detection and correction initialization */
2111 if(fi->walking && fi->canPenetrate)
2112 {
2113 /* set up avatar to last valid avatar position vector in avatar space */
2114 double plen = 0.0;
2115 struct point_XYZ lastpos;
2116 lastpos = viewer_lastP_get(); /* in viewer/avatar space */
2117 transform(&lastpos,&lastpos,fi->avatar2collision); /* convert to collision space */
2118 /* if vector length == 0 can't penetrate - don't bother to check */
2119 plen = sqrt(vecdot(&lastpos,&lastpos));
2120 if(APPROX(plen,0.0))
2121 fi->canPenetrate = 0;
2122 else
2123 {
2124 /* precompute MBB/extent etc in collision space for penetration vector */
2125 struct point_XYZ pos = {0.0,0.0,0.0};
2126 fi->penMin[0] = DOUBLE_MIN(pos.x,lastpos.x);
2127 fi->penMin[1] = DOUBLE_MIN(pos.y,lastpos.y);
2128 fi->penMin[2] = DOUBLE_MIN(pos.z,lastpos.z);
2129 fi->penMax[0] = DOUBLE_MAX(pos.x,lastpos.x);
2130 fi->penMax[1] = DOUBLE_MAX(pos.y,lastpos.y);
2131 fi->penMax[2] = DOUBLE_MAX(pos.z,lastpos.z);
2132 fi->penRadius = plen;
2133 vecnormal(&lastpos,&lastpos);
2134 fi->penvec = lastpos;
2135 fi->pendisp = 0.0; /* used to sort penetration intersections to pick one closest to last valid avatar position */
2136 }
2137 }
2138
2139 render_hier(rootNode(), VF_Collision);
2140 if(!fi->isPenetrate)
2141 {
2142 /* we don't clear if we just solved a penetration, otherwise we'll get another penetration going back, from the correction.
2143 No pen? Then clear here to start over on the next loop.
2144 */
2145 viewer_lastP_clear();
2146 }
2147 get_collisionoffset(&(v.x), &(v.y), &(v.z));
2148
2149 /* if (!APPROX(v.x,0.0) || !APPROX(v.y,0.0) || !APPROX(v.z,0.0)) {
2150 printf ("%lf MainLoop, rendercollisions, offset %f %f %f\n",TickTime(),v.x,v.y,v.z);
2151 } */
2152 /* v should be in avatar coordinates*/
2153 increment_pos(&v);
2154}
2155
2156
2157
2158#ifdef DEBUG_SCENE_EXPORT
2159void printpolyrep(struct X3D_PolyRep pr) {
2160 int i;
2161 int npoints = 0;
2162 printf("X3D_PolyRep makepolyrep() {\n");
2163 printf(" int cindext[%d] = {",pr.ntri*3);
2164 for(i=0; i < pr.ntri*3-1; i++) {
2165 printf("%d,",pr.cindex[i]);
2166 if(pr.cindex[i] > npoints)
2167 npoints = pr.cindex[i];
2168 }
2169 printf("%d};\n",pr.cindex[i]);
2170 if(pr.cindex[i] > npoints)
2171 npoints = pr.cindex[i];
2172
2173 printf(" float coordt[%d] = {",npoints*3);
2174 for(i=0; i < npoints*3-1; i++)
2175 printf("%f,",pr.actualCoord[i]);
2176 printf("%f};\n",pr.actualCoord[i]);
2177
2178 printf("static int cindex[%d];\nstatic float coord[%d];\n",pr.ntri*3,npoints*3);
2179 printf("X3D_PolyRep pr = {0,%d,cindex,coord,NULL,NULL,NULL,NULL,NULL,NULL};\n",pr.ntri);
2180 printf("memcpy(cindex,cindext,sizeof(cindex));\n");
2181 printf("memcpy(coord,coordt,sizeof(coord));\n");
2182 printf("return pr; }\n");
2183
2184};
2185
2186void printmatrix(GLDOUBLE* mat) {
2187 int i;
2188 printf("void getmatrix(GLDOUBLE* mat, struct point_XYZ disp) {\n");
2189 for(i = 0; i< 16; i++) {
2190 printf("mat[%d] = %f%s;\n",i,mat[i],i==12 ? " +disp.x" : i==13? " +disp.y" : i==14? " +disp.z" : "");
2191 }
2192 printf("}\n");
2193
2194}
2195#endif
2196