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#include "Polyrep.h"
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 = {.x=0,.y=0,.z=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 = { .x = 0,.y = 0,.z = 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 = {.x=0,.y=0,.z=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= { .x = 0,.y = 0,.z = 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= { .x = 0,.y = 0,.z = 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) {
1641//static struct point_XYZ polyrep_disp_rec2(struct X3D_PolyRep* pr, struct point_XYZ* n, struct point_XYZ dispsum, prflags flags) {
1642static struct point_XYZ polyrep_disp_rec2(float *coord, int* cindex, int ntri, int ccw, struct point_XYZ* n, struct point_XYZ dispsum, prflags flags) {
1643 struct point_XYZ p[3];
1644 double maxdisp = 0;
1645 struct point_XYZ maxdispv = { .x = 0,.y = 0,.z = 0 };
1646 double disp;
1647 struct point_XYZ dispv;
1648 int i;
1649 int frontfacing;
1650 //int ccw;
1651
1652 //ccw = pr->ccw;
1653
1654//printf ("start polyrep_disp_rec2\n");
1655
1656 for(i = 0; i < ntri; i++)
1657 {
1658 p[0].x = coord[cindex[i*3]*3] +dispsum.x;
1659 p[0].y = coord[cindex[i*3]*3+1] +dispsum.y;
1660 p[0].z = coord[cindex[i*3]*3+2] +dispsum.z;
1661
1662 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 */
1663 else frontfacing = (vecdot(&n[i],&p[0]) >= 0); /*if ccw facing avatar */
1664
1665 /* printf ("polyrep_disp_rec, frontfacing %d BACKFACING %d FRONTFACING %d DOUBLESIDED %d\n",
1666 frontfacing, flags & PR_BACKFACING, flags & PR_FRONTFACING, flags & PR_DOUBLESIDED); */
1667 /* use if either:
1668 -frontfacing and not in doubleside mode;
1669 -if in doubleside mode:
1670 use if either:
1671 -PR_FRONTFACING or PR_BACKFACING not yet specified
1672 -fontfacing and PR_FRONTFACING specified
1673 -not frontfacing and PR_BACKFACING specified
1674 */
1675 if( (frontfacing && !(flags & PR_DOUBLESIDED) )
1676 || ( (flags & PR_DOUBLESIDED) && !(flags & (PR_FRONTFACING | PR_BACKFACING) ) )
1677 || (frontfacing && (flags & PR_FRONTFACING))
1678 || (!frontfacing && (flags & PR_BACKFACING)) )
1679 {
1680
1681 struct point_XYZ nused;
1682 p[1].x = coord[cindex[i*3+1]*3] +dispsum.x;
1683 p[1].y = coord[cindex[i*3+1]*3+1] +dispsum.y;
1684 p[1].z = coord[cindex[i*3+1]*3+2] +dispsum.z;
1685 p[2].x = coord[cindex[i*3+2]*3] +dispsum.x;
1686 p[2].y = coord[cindex[i*3+2]*3+1] +dispsum.y;
1687 p[2].z = coord[cindex[i*3+2]*3+2] +dispsum.z;
1688
1689 if(frontfacing)
1690 {
1691 nused = n[i];
1692 } else { /*can only be here in DoubleSided mode*/
1693 /*reverse polygon orientation, and do calculations*/
1694 vecscale(&nused,&n[i],-1.0);
1695 }
1696 /* ready to start testing the triangle against our collision volume (sphere/cylinder/line) tests.
1697 The avatar collision volume -
1698 A.for flying/examining:
1699 1. sphere.
1700
1701 B.For walking:
1702 1. sphere (head) <<<<< problem I do not like this displacement logic when walking
1703 because you'll end up floating over something that's just below avatar 0,0,0
1704 which for walking is wrong - you should collide with the cylinder going from top to bottom.
1705 Recommendation: either always do both sphere and cylinder or
1706 just do #2 cylinder abottom to atop for head and body.
1707 2. cylinder (body)
1708 3. line - climb/fall (legs,feet)
1709 Order of tests (so can exit early to save unnecessary computations):
1710 sphere > cylinder > climb > fall
1711 if you have a sphere hit you don't need to test the rest.
1712 If you have a cylinder hit, you don't need to test climb/fall.
1713 if you have a climb, you can skip the fall.
1714 otherwise test for fall.
1715
1716 fast tests using MBB in the caller elliminate some volumes
1717 docollision - sphere and cyclinder should be tested
1718 dofall - climb and fall should be tested
1719
1720 Total logic for walking:
1721 collision = docollision
1722 if(docollision)
1723 do sphere
1724 if not sphere
1725 do cylinder
1726 if not cylinder
1727 collision = 0
1728 if( dofall && !collision )
1729 do line
1730 if not climb
1731 do fall
1732 */
1733 // printf ("calling get_poly_disp_2 at %s:%d\n",__FILE__,__LINE__);
1734
1735 dispv = get_poly_disp_2(p, 3, nused); //get_poly_disp_2(y1,y2,ystep,r, p, 3, nused);
1736 disp = vecdot(&dispv,&dispv);
1737
1738
1739 #ifdef DEBUGPTS
1740 if(dispv.x != 0 || dispv.y != 0 || dispv.z != 0)
1741 printf("polyd: (%f,%f,%f) |%f|\n",dispv.x,dispv.y,dispv.z,disp);
1742 #endif
1743
1744
1745 /*keep result only if:
1746 displacement is positive
1747 displacement is smaller than minimum displacement up to date
1748 */
1749 if( (disp > FLOAT_TOLERANCE) && (disp > maxdisp) )
1750 {
1751 maxdisp = disp;
1752 maxdispv = dispv;
1753 }
1754 }
1755 }
1756
1757 VECADD(dispsum,maxdispv);
1758 //printf ("end polyrep_disp_rec2, dispsum %lf %lf %lf at end\n",dispsum.x,dispsum.y,dispsum.z);
1759 return dispsum;
1760}
1761
1762#undef POLYREP_DISP2_PERFORMANCE
1763#ifdef POLYREP_DISP2_PERFORMANCE
1764
1765static bool timing = FALSE;
1766static double startTime = 0.0;
1767static double stopTime = 0.0;
1768static double accumTime = 0.0;
1769static int counter = 0;
1770
1771#endif
1772
1773/*uses sphere displacement, and a cylinder for stepping
1774 y1, y2, ystep, r - (usually abottom, atop, astep, awidth) are from naviiinfo avatar height, step, width
1775 pr - will be transformed by mat from raw shape coordinates into collision space:
1776 Fly/Examine: avatar space
1777 Walk: Bound-Viewpoint-Vertical-aligned Avatar-centric BVVA space.
1778 flags -
1779*/
1780struct point_XYZ polyrep_disp2(struct X3D_PolyRep *pr, GLDOUBLE* mat, prflags flags) {
1781 int i;
1782 unsigned int maxc;
1783 ppcollision pp = (ppcollision)gglobal()->collision.prv;
1784
1785#ifdef POLYREP_DISP2_PERFORMANCE
1786 if (!timing) {
1787 printf ("start timing polyrep_disp2\n");
1788 timing = TRUE;
1789
1790 }
1791 startTime = Time1970sec();
1792#endif
1793
1794
1795#ifdef HAVE_OPENCL
1796
1797 if ((pr.VBO_buffers[VERTEX_VBO] != 0) && pp->OpenCL_Collision_Program_initialized) {
1798 ttglobal tg = gglobal();
1799 float awidth = (float) tg->Bindable.naviinfo.width; /*avatar width*/
1800
1801 float mymat[16];
1802 for (i=0; i<16; i++) {
1803 mymat[i] = (float) mat[i];
1804 }
1805
1806 pp->res = run_non_walk_collide_program(
1807 pr.VBO_buffers[VERTEX_VBO],pr.VBO_buffers[INDEX_VBO],mymat, pr.ntri,
1808 pr.ccw, flags, awidth);
1809
1810 //printf ("openCL sez: move us %f %f %f\n",pp->res.x,pp->res.y,pp->res.z);
1811
1812
1813#ifdef POLYREP_DISP2_PERFORMANCE
1814 stopTime = Time1970sec();
1815 accumTime += stopTime - startTime;
1816
1817 if (counter == 25) {
1818 printf ("polyrep_disp2, averaged over 25 runs: %f\n",
1819 accumTime/25.0);
1820 counter = 0;
1821 accumTime = 0.0;
1822 }
1823
1824 counter ++;
1825#endif
1826
1827 return pp->res;
1828 }
1829
1830 /* if we are here, there was an OpenCL issue and we have to do this by software */
1831#endif
1832
1833
1834 pp->res.x=0.0; pp->res.y=0.0; pp->res.z=0.0;
1835 maxc = 0; /* highest cindex, used to point into prd_newc_floats structure.*/
1836
1837 for(i = 0; i < pr->ntri*3; i++) {
1838 if (pr->cindex[i] > maxc) {maxc = pr->cindex[i];}
1839 }
1840
1841 /*transform all points from raw shape to viewer(fly) or BVAAC(walk) space */
1842 if (maxc> pp->prd_newc_floats_size) {
1843 pp->prd_newc_floats = REALLOC(pp->prd_newc_floats,maxc*9*sizeof(float));
1844 pp->prd_newc_floats_size = maxc;
1845 }
1846
1847
1848 for(i = 0; i < pr->ntri*3; i++) {
1849 transformf(&pp->prd_newc_floats[pr->cindex[i]*3],&pr->actualCoord[pr->cindex[i]*3],mat);
1850 }
1851
1852 //pr->actualCoord = pp->prd_newc_floats; /*remember, coords are only replaced in our local copy of PolyRep */
1853
1854
1855 /*pre-calculate face normals */
1856 if (pr->ntri> pp->prd_normals_size) {
1857 pp->prd_normals = REALLOC(pp->prd_normals,pr->ntri*sizeof(struct point_XYZ));
1858 pp->prd_normals_size = pr->ntri;
1859 }
1860
1861 for(i = 0; i < pr->ntri; i++) {
1862 polynormalf(&pp->prd_normals[i],&pp->prd_newc_floats[pr->cindex[i*3]*3],
1863 &pp->prd_newc_floats[pr->cindex[i*3+1]*3],&pp->prd_newc_floats[pr->cindex[i*3+2]*3]);
1864 }
1865
1866 //pp->res = polyrep_disp_rec2(pr,pp->prd_normals,pp->res,flags); //polyrep_disp_rec(y1,y2,ystep,r,&pr,prd_normals,res,flags);
1867 pp->res = polyrep_disp_rec2(pp->prd_newc_floats, pr->cindex, pr->ntri, pr->ccw, pp->prd_normals, pp->res, flags);
1868 /* printf ("polyrep_disp_rec2 tells us to move: %f %f %f\n",pp->res.x, pp->res.y, pp->res.z); */
1869
1870 //pr->actualCoord = 0;
1871
1872#ifdef POLYREP_DISP2_PERFORMANCE
1873 stopTime = Time1970sec();
1874 accumTime += stopTime - startTime;
1875
1876 if (counter == 25) {
1877 printf ("polyrep_disp2, averaged over 25 runs: %f\n",
1878 accumTime/25.0);
1879 counter = 0;
1880 accumTime = 0.0;
1881 }
1882
1883 counter ++;
1884#endif
1885
1886 return pp->res;
1887}
1888
1889
1890
1891
1892/*Optimized polyrep_disp for planar polyreps.
1893 Used for text.
1894 planar_polyrep_disp computes the normal using the first polygon, if no normal is specified (if it is zero).
1895 JAS - Normal is always specified now. (see VRMLRend.pm for invocation)
1896*/
1897static struct point_XYZ planar_polyrep_disp_rec(double y1, double y2, double ystep, double r, float *coord, int *cindex, int ntri, struct point_XYZ n, struct point_XYZ dispsum, prflags flags) {
1898 struct point_XYZ p[3];
1899 double lmaxdisp = 0;
1900 struct point_XYZ maxdispv = { .x = 0,.y = 0,.z = 0 };
1901 double disp;
1902 struct point_XYZ dispv;
1903 /* static int recursion_count = 0; */
1904 int i;
1905 int frontfacing;
1906 ppcollision pp = (ppcollision)gglobal()->collision.prv;
1907
1908 p[0].x = coord[cindex[0]*3] +dispsum.x;
1909 p[0].y = coord[cindex[0]*3+1] +dispsum.y;
1910 p[0].z = coord[cindex[0]*3+2] +dispsum.z;
1911
1912 frontfacing = (vecdot(&n,&p[0]) < 0); /*if normal facing avatar */
1913
1914 if(!frontfacing && !(flags & PR_DOUBLESIDED)) return dispsum;
1915
1916 if(!frontfacing) vecscale(&n,&n,-1.0);
1917
1918 for(i = 0; i < ntri; i++) {
1919 p[0].x = coord[cindex[i*3]*3] +dispsum.x;
1920 p[0].y = coord[cindex[i*3]*3+1] +dispsum.y;
1921 p[0].z = coord[cindex[i*3]*3+2] +dispsum.z;
1922 p[1].x = coord[cindex[i*3+1]*3] +dispsum.x;
1923 p[1].y = coord[cindex[i*3+1]*3+1] +dispsum.y;
1924 p[1].z = coord[cindex[i*3+1]*3+2] +dispsum.z;
1925 p[2].x = coord[cindex[i*3+2]*3] +dispsum.x;
1926 p[2].y = coord[cindex[i*3+2]*3+1] +dispsum.y;
1927 p[2].z = coord[cindex[i*3+2]*3+2] +dispsum.z;
1928
1929 //dispv = get_poly_disp(y1,y2,ystep, r, p, 3, n);
1930 dispv = get_poly_disp_2(p, 3, n);
1931 disp = -pp->get_poly_mindisp; /*global variable. was calculated inside poly_normal_disp already. */
1932
1933#ifdef DEBUGPTS
1934 printf("polyd: (%f,%f,%f) |%f|\n",dispv.x,dispv.y,dispv.z,disp);
1935#endif
1936
1937 /*keep result only if:
1938 displacement is positive
1939 displacement is bigger than maximum displacement up to date
1940 */
1941 if((disp > FLOAT_TOLERANCE) && (disp > lmaxdisp)) {
1942 lmaxdisp = disp;
1943 maxdispv = dispv;
1944 }
1945
1946 }
1947 VECADD(dispsum,maxdispv);
1948 return dispsum;
1949
1950}
1951
1952
1953struct 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) {
1954 int i;
1955 unsigned int maxc;
1956 ppcollision pp = (ppcollision)gglobal()->collision.prv;
1957
1958
1959 pp->res.x=0.0; pp->res.y=0.0; pp->res.z=0.0;
1960 maxc = 0; /* highest cindex, used to point into newc structure.*/
1961
1962 for(i = 0; i < pr->ntri*3; i++) {
1963 if (pr->cindex[i] > maxc) {maxc = pr->cindex[i];}
1964 }
1965
1966 /*transform all points to viewer space */
1967 if (maxc> pp->prd_newc_floats_size) {
1968 pp->prd_newc_floats = REALLOC(pp->prd_newc_floats,maxc*9*sizeof(float));
1969 pp->prd_newc_floats_size = maxc;
1970 }
1971
1972 for(i = 0; i < pr->ntri*3; i++) {
1973 transformf(&pp->prd_newc_floats[pr->cindex[i]*3],&pr->actualCoord[pr->cindex[i]*3],mat);
1974 }
1975 //pr->actualCoord = pp->prd_newc_floats; /*remember, coords are only replaced in our local copy of PolyRep */
1976
1977 /*if normal not speced, calculate it */
1978 /* if(n.x == 0 && n.y == 0 && n.z == 0.) */
1979 if(APPROX(n.x, 0) && APPROX(n.y, 0) && APPROX(n.z, 0)) {
1980 polynormalf(&n,&pp->prd_newc_floats[pr->cindex[0]*3],&pp->prd_newc_floats[pr->cindex[1]*3],&pp->prd_newc_floats[pr->cindex[2]*3]);
1981 }
1982
1983 pp->res = planar_polyrep_disp_rec(y1,y2,ystep,r,pp->prd_newc_floats,pr->cindex,pr->ntri,n,pp->res,flags);
1984
1985 return pp->res;
1986}
1987
1988
1989
1990
1991
1992
1993static void get_collisionoffset(double *x, double *y, double *z)
1994{
1995 struct sCollisionInfo *ci;
1996 struct sFallInfo *fi;
1997 struct sNaviInfo *naviinfo;
1998 struct point_XYZ xyz;
1999 struct point_XYZ res;
2000 ttglobal tg = gglobal();
2001 ci = CollisionInfo();
2002 fi = FallInfo();
2003 naviinfo = (struct sNaviInfo*)tg->Bindable.naviinfo;
2004 res = ci->Offset;
2005 /* collision.offset should be in collision space coordinates: fly/examine: avatar space, walk: BVVA space */
2006 /* uses mean direction, with maximum distance */
2007
2008 /* xyz is in collision space- fly/examine: avatar space, walk: BVVA space */
2009 xyz.x = xyz.y = xyz.z = 0.0;
2010
2011 if(ci->Count > 0 && !APPROX(vecnormal(&res, &res),0.0) )
2012 vecscale(&xyz, &res, sqrt(ci->Maximum2));
2013
2014 /* for WALK + collision */
2015 if(fi->walking)
2016 {
2017 if(fi->canFall && fi->isFall )
2018 {
2019 /* canFall == true if we aren't climbing, isFall == true if there's no climb, and there's geom to fall to */
2020 double floatfactor = .1;
2021 if(fi->allowClimbing) floatfactor = 0.0; /*popcycle method */
2022 if(fi->smoothStep){
2023 //its socially acceptable to float a bit when falling...
2024 double fallstep = DOUBLE_MIN(fi->hfall,fi->hfall * 2.0 * (TickTime() - lastTime()));
2025 //if(0) xyz.y = DOUBLE_MAX(fi->hfall,-fi->fallStep) + naviinfo->height*floatfactor; //pre-2018 method
2026 xyz.y = DOUBLE_MAX(fi->hfall,fallstep); //+ naviinfo->height*floatfactor;
2027 }else{
2028 xyz.y = fi->hfall + naviinfo->height*floatfactor; //.1;
2029 }
2030 }
2031 if(fi->isClimb && fi->allowClimbing)
2032 {
2033 /* stepping up normally handled by cyclindrical collision, but there are settings to use this climb instead */
2034 //but when climbing its not cool to dig underneath the terrain, so assymmetrical, a bit faster climbing
2035 //than falling.
2036 if(fi->smoothStep && FALSE){
2037 double fallstep = DOUBLE_MIN(fi->hclimb, fi->hclimb * 8.0 * (TickTime() - lastTime()));
2038 //if(0) xyz.y = DOUBLE_MIN(fi->hclimb,fi->fallStep); //pre-2018 method
2039 xyz.y = DOUBLE_MIN(fi->hclimb,fallstep);
2040 }else{
2041 xyz.y = fi->hclimb;
2042 }
2043 }
2044 if(fi->isPenetrate)
2045 {
2046 /*over-ride everything else*/
2047 xyz = fi->pencorrection;
2048 }
2049 }
2050 /* now convert collision-space deltas to avatar space via collision2avatar- fly/examine: identity (do nothing), walk:BVVA2A */
2051 transform3x3(&xyz,&xyz,fi->collision2avatar);
2052 /* now xyz is in avatar space, ready to be added to avatar viewer.pos */
2053 *x = xyz.x;
2054 *y = xyz.y;
2055 *z = xyz.z;
2056 /* another transform possible: from avatar space into navigation space. fly/examine: identity walk: A2BVVA*/
2057}
2058struct point_XYZ viewer_lastP_get();
2059void render_collisions(int Viewer_type) {
2060 struct point_XYZ v;
2061 struct sCollisionInfo *ci;
2062 struct sFallInfo *fi;
2063 struct sNaviInfo *naviinfo;
2064 naviinfo = (struct sNaviInfo*)gglobal()->Bindable.naviinfo;
2065
2066 if(!(Viewer_type == VIEWER_WALK || Viewer_type == VIEWER_FLY)) return; //no collisions
2067 ci = CollisionInfo();
2068 fi = FallInfo();
2069 ci->Offset.x = 0;
2070 ci->Offset.y = 0;
2071 ci->Offset.z = 0;
2072 ci->Count = 0;
2073 ci->Maximum2 = 0.;
2074
2075 /* popcycle shaped avatar collision volume: ground to stepheight is a ray, stepheight to avatarheight is a cylinder,
2076 and a sphere on top?
2077 -keeps cyclinder from dragging in terrain mesh, easy to harmonize fall and climb math so not fighting (now a ray both ways)
2078 -2 implementations: analytical cyclinder, sampler
2079 The sampler method intersects line segments radiating from the the avatar axis with shape facets - misses small shapes but good
2080 for walls and floors; intersection math is simple: line intersect plane.
2081 */
2082 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 */
2083 fi->climbHeight = 100.0*naviinfo->height; // 200.0; //sometimes you get underneath the terrain. At what point should we cimb you out automatically
2084 fi->fallStep = 1.0; /* maximum height to fall on one frame */
2085 fi->walking = Viewer_type == VIEWER_WALK; //viewer_type == VIEWER_WALK;
2086 fi->canFall = fi->walking; /* && COLLISION (but we wouldn't be in here if not). Will be set to 0 if a climb is found. */
2087 fi->hits = 0; /* counter (number of vertical hits) set to zero here once per frame */
2088 fi->isFall = 0; /*initialize here once per frame - flags if there's a fall found */
2089 fi->isClimb = 0; /* initialize here each frame */
2090 fi->smoothStep = 1; /* [1] setting - will only fall by fallstep on a frame rather than the full hfall */
2091 fi->allowClimbing = 1; /* [0] - setting - 0=climbing done by collision with cyclinder 1=signals popcycle avatar collision volume and allows single-footpoint climbing */
2092
2093 fi->canPenetrate = 1; /* setting - 0= don't check for wall penetration 1= check for wall penetration */
2094 fi->isPenetrate = 0; /* set to zero once per loop and will come back 1 if there was a penetration detected and corrected */
2095
2096 /* at this point we know the navigation mode and the pose of the avatar, and of the boundviewpoint
2097 so pre-compute some handy matrices for collision calcs - the avatar2collision and back (a tilt matrix for gravity direction)
2098 */
2099 if(fi->walking)
2100 {
2101 /*bound viewpoint vertical aligned gravity as per specs*/
2102 avatar2BoundViewpointVerticalAvatar(fi->avatar2collision, fi->collision2avatar);
2103 }
2104 else
2105 {
2106 /* when flying or examining, no gravity - up is your avatar's up */
2107 loadIdentityMatrix(fi->avatar2collision);
2108 loadIdentityMatrix(fi->collision2avatar);
2109 }
2110
2111 /* wall penetration detection and correction initialization */
2112 if(fi->walking && fi->canPenetrate)
2113 {
2114 /* set up avatar to last valid avatar position vector in avatar space */
2115 double plen = 0.0;
2116 struct point_XYZ lastpos;
2117 lastpos = viewer_lastP_get(); /* in viewer/avatar space */
2118 transform(&lastpos,&lastpos,fi->avatar2collision); /* convert to collision space */
2119 /* if vector length == 0 can't penetrate - don't bother to check */
2120 plen = sqrt(vecdot(&lastpos,&lastpos));
2121 if(APPROX(plen,0.0))
2122 fi->canPenetrate = 0;
2123 else
2124 {
2125 /* precompute MBB/extent etc in collision space for penetration vector */
2126 struct point_XYZ pos = {.x=0,.y=0,.z=0};
2127 fi->penMin[0] = DOUBLE_MIN(pos.x,lastpos.x);
2128 fi->penMin[1] = DOUBLE_MIN(pos.y,lastpos.y);
2129 fi->penMin[2] = DOUBLE_MIN(pos.z,lastpos.z);
2130 fi->penMax[0] = DOUBLE_MAX(pos.x,lastpos.x);
2131 fi->penMax[1] = DOUBLE_MAX(pos.y,lastpos.y);
2132 fi->penMax[2] = DOUBLE_MAX(pos.z,lastpos.z);
2133 fi->penRadius = plen;
2134 vecnormal(&lastpos,&lastpos);
2135 fi->penvec = lastpos;
2136 fi->pendisp = 0.0; /* used to sort penetration intersections to pick one closest to last valid avatar position */
2137 }
2138 }
2139
2140 render_hier(rootNode(), VF_Collision);
2141 if(!fi->isPenetrate)
2142 {
2143 /* we don't clear if we just solved a penetration, otherwise we'll get another penetration going back, from the correction.
2144 No pen? Then clear here to start over on the next loop.
2145 */
2146 viewer_lastP_clear();
2147 }
2148 get_collisionoffset(&(v.x), &(v.y), &(v.z));
2149
2150 /* if (!APPROX(v.x,0.0) || !APPROX(v.y,0.0) || !APPROX(v.z,0.0)) {
2151 printf ("%lf MainLoop, rendercollisions, offset %f %f %f\n",TickTime(),v.x,v.y,v.z);
2152 } */
2153 /* v should be in avatar coordinates*/
2154 increment_pos(&v);
2155}
2156
2157
2158
2159#ifdef DEBUG_SCENE_EXPORT
2160void printpolyrep(struct X3D_PolyRep pr) {
2161 int i;
2162 int npoints = 0;
2163 printf("X3D_PolyRep makepolyrep() {\n");
2164 printf(" int cindext[%d] = {",pr.ntri*3);
2165 for(i=0; i < pr.ntri*3-1; i++) {
2166 printf("%d,",pr.cindex[i]);
2167 if(pr.cindex[i] > npoints)
2168 npoints = pr.cindex[i];
2169 }
2170 printf("%d};\n",pr.cindex[i]);
2171 if(pr.cindex[i] > npoints)
2172 npoints = pr.cindex[i];
2173
2174 printf(" float coordt[%d] = {",npoints*3);
2175 for(i=0; i < npoints*3-1; i++)
2176 printf("%f,",pr.actualCoord[i]);
2177 printf("%f};\n",pr.actualCoord[i]);
2178
2179 printf("static int cindex[%d];\nstatic float coord[%d];\n",pr.ntri*3,npoints*3);
2180 printf("X3D_PolyRep pr = {0,%d,cindex,coord,NULL,NULL,NULL,NULL,NULL,NULL};\n",pr.ntri);
2181 printf("memcpy(cindex,cindext,sizeof(cindex));\n");
2182 printf("memcpy(coord,coordt,sizeof(coord));\n");
2183 printf("return pr; }\n");
2184
2185};
2186
2187void printmatrix(GLDOUBLE* mat) {
2188 int i;
2189 printf("void getmatrix(GLDOUBLE* mat, struct point_XYZ disp) {\n");
2190 for(i = 0; i< 16; i++) {
2191 printf("mat[%d] = %f%s;\n",i,mat[i],i==12 ? " +disp.x" : i==13? " +disp.y" : i==14? " +disp.z" : "");
2192 }
2193 printf("}\n");
2194
2195}
2196#endif
2197