33#include <libFreeWRL.h>
36#include "../x3d_parser/Bindable.h"
37#include "RenderFuncs.h"
39#include "../vrml_parser/Structs.h"
40#include "../main/headers.h"
42#include "LinearAlgebra.h"
44#include "../opencl/OpenCL_Utils.h"
47#include "../internal.h"
52void accumulateFallingClimbing(
double y1,
double y2,
double ystep,
struct point_XYZ *p,
int num,
struct point_XYZ nused,
double *tmin,
double *tmax);
57#define DJ_KEEP_COMPILER_WARNING 0
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
70#define STEPUP_MAXINCLINE 0.9
73#define DEBUGPTSPRINT(x,y,z) printf(x,y,z)
75#define DEBUGPTSPRINT(x,y,z) {}
78static const struct point_XYZ zero = {0,0,0};
81 float* prd_newc_floats;
82 unsigned int prd_newc_floats_size;
99 struct sCollisionGPU CollisionGPU;
105 double get_poly_mindisp;
110 bool OpenCL_Collision_Program_initialized;
113void *collision_constructor(){
118void collision_init(
struct tcollision *t){
122 t->prv = collision_constructor();
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;
131 p->clippedPoly2 = NULL;
132 p->clippedPoly2Size = 0;
133 p->clippedPoly3 = NULL;
134 p->clippedPoly3Size = 0;
135 p->clippedPoly4 = NULL;
136 p->clippedPoly4Size = 0;
137 p->clippedPoly5 = NULL;
138 p->clippedPoly5Size = 0;
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;
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;
159 p->OpenCL_Collision_Program_initialized = FALSE;
164void collision_clear(
struct tcollision *t){
169 FREE_IF_NZ(p->prd_newc_floats);
170 FREE_IF_NZ(p->prd_normals);
178 return &p->CollisionInfo;
190void createGPUCollisionProgram () {
193 p->OpenCL_Collision_Program_initialized = collision_initGPUCollide(&p->CollisionGPU);
196struct sCollisionGPU* GPUCollisionInfo()
199 return &p->CollisionGPU;
204#define make_pt(p,xc,yc,zc) { p.x = (xc); p.y = (yc); p.z = (zc); }
206int overlapMBBs(GLDOUBLE *MBBmin1, GLDOUBLE *MBBmax1, GLDOUBLE *MBBmin2, GLDOUBLE* MBBmax2)
218 overlap = overlap && !(MBBmin1[i] > MBBmax2[i] || MBBmax1[i] < MBBmin2[i]);
227 double len2 = vecdot(&add,&add);
229 VECADD(ci->Offset,add);
230 if(len2 > ci->Maximum2)
234static double closest_point_of_segment_to_origin(
struct point_XYZ p1,
struct point_XYZ p2) {
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 );
242 double i = ((APPROX(q, 0)) ? 0.5 : (p1.x * x12 + p1.y * y12 + p1.z * z12) / q);
257 double k = b.x*n.x + b.y*n.y + b.z*n.z;
277 if(APPROX(q2.x, 0) && APPROX(q2.z, 0)) {
285 quotient = ((-p2.z)*q2.x + p2.x*q2.z);
287 if(APPROX(quotient, 0))
return 0;
289 k = (p1.z*q2.x - q1.z*q2.x - p1.x*q2.z + q1.x*q2.z)/quotient;
292 if((k >= 0.) && (k < 1.)) {
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;
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);
316 *k1 = (b+sqrdelta)/a;
318 if(APPROX(sqrdelta, 0))
return 1;
320 *k2 = (b-sqrdelta)/a;
329 make_pt(ret,p1.x - (n.x*(p1.y-y))/n.y,y,(p1.z - (n.z*(p1.y-y))/n.y));
339 vecscale(&n,&n,-1.0);
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);
355static int perpendicular_line_passing_inside_poly(
struct point_XYZ a,
struct point_XYZ* p,
int num) {
365 if(APPROX(vecnormal(&n,&a), 0)) {
369 make_orthogonal_vector_space(&i,&j,n);
371 vecscale(&epsilon,&j,FLOAT_TOLERANCE);
373 for(f = 0; f < num; f++) {
378 VECDIFF(p[(f+1)%num],a,p2);
380 while(APPROX((p1j = vecdot(&p1,&j)), 0)) VECADD(p1,epsilon);
383 while(APPROX((p2j = vecdot(&p2,&j)), 0)) VECADD(p2,epsilon);
386 if(p1j * p2j <= 0 ) {
391 k = (! APPROX(p1j-p2j, 0)) ? (p1j/ (p1j - p2j)) : 0.;
394 p0 = weighted_sum(p1, p2, k);
395 if(vecdot(&p0,&i) >= 0)
401 return sectcount % 2;
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;
414 VECDIFF(pp2,pp1,pp2);
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);
424 *k1 = (b+sqrdelta)/a;
425 *k2 = (b-sqrdelta)/a;
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);
459 vecdiff(&a,&D,&p[i]);
460 vecdiff(&b,&p[j],&p[i]);
463 inside = inside && vecdot(&last,&c) >= 0.0;
493 d = -vecdot(&n,&p[0]);
496 ndotD = vecdot(&n,&D);
498 if(APPROX(ndotD,0.0) )
504 t = - ( (d + vecdot(&n,&s0)) / ndotD );
506 if( t < 0.0 || t > r )
515 hit = pointOnPlaneInsidePoly(D,p,num,&n);
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)
528 double level[3],dr,dmax,eighth,theta, avmin[3], avmax[3];
533 s0.x = s0.y = s0.z = 0.0;
535 eighth = M_PI * 2.0 / 8.0;
542 avmin[1] = ystep; avmax[1] = y2;
543 if(!overlapMBBs(avmin,avmax,tmin,tmax))
return result;
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) )
565 hit = intersectLineSegmentWithPoly(s0,s1,r,p,num,n,&dr);
568 if( (dr > FLOAT_TOLERANCE) && (dr > dmax) )
571 vecscale(&result,&s1,r-dr);
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)
611 if( overlapMBBs(fi->penMin,fi->penMax,tmin,tmax) )
614 hit = intersectLineSegmentWithPoly(s0,fi->penvec,fi->penRadius,p,num,n,&dr);
617 vecscale(result,&fi->penvec,(dr + r));
654 double tmin[3],tmax[3];
657 GLDOUBLE awidth, atop, abottom, astep;
660 naviinfo = (
struct sNaviInfo *)tg->Bindable.naviinfo;
661 awidth = naviinfo->width;
662 atop = naviinfo->width;
663 abottom = -naviinfo->height;
664 astep = -naviinfo->height+naviinfo->step;
668 pp->get_poly_mindisp = 0.0;
673 tmin[0] = tmax[0] = p[0].x;
674 tmin[1] = tmax[1] = p[0].y;
675 tmin[2] = tmax[2] = p[0].z;
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);
690 if(fi->checkPenetration)
694 hit = get_poly_penetration_disp(awidth,p,num,n,tmin,tmax,&presult,&rmax);
698 if(rmax > fi->pendisp)
700 fi->pencorrection = presult;
705 if(fi->checkCylinder && !hit)
707 result = get_poly_radialSample_disp(abottom,atop,astep,awidth,p,num,n,tmin,tmax);
709 hit = !(APPROX(result.x, 0) && APPROX(result.y, 0) && APPROX(result.z, 0));
713 if(fi->checkFall && !hit)
715 accumulateFallingClimbing(abottom,atop,astep,p,num,n,tmin,tmax);
723 result = get_poly_min_disp_with_sphere(awidth, p, num, n);
725 pp->get_poly_mindisp = vecdot(&result,&result);
738 double tmin[3],tmax[3],rmin[3],rmax[3],q[3];
739 double get_poly_mindisp;
740 int clippedPoly4num = 0;
742 get_poly_mindisp = 1E90;
750 memcpy(tmin,&p[0],3*
sizeof(
double));
751 memcpy(tmax,&p[num-1],3*
sizeof(
double));
755 memcpy(q,&p[i],3*
sizeof(
double));
758 tmin[j] = DOUBLE_MIN(tmin[j],q[j]);
759 tmax[j] = DOUBLE_MAX(tmax[j],q[j]);
768 if( !overlapMBBs(rmin,rmax,tmin,tmax) )
775 if(facemask != debugsurface++)
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;
786 if(APPROX(n.x, 0) && APPROX(n.y, 0) && APPROX(n.z, 0)) {
787 polynormal(&n,&p[0],&p[1],&p[2]);
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]));
797 pp->clippedPoly4[clippedPoly4num] = closest_point_of_plane_to_origin(p[0],n);
811 if(perpendicular_line_passing_inside_poly(pp->clippedPoly4[clippedPoly4num],p, num)) {
812 DEBUGPTSPRINT(
"perpendicular_line_passing_inside_poly[%d]= %d\n",0,clippedPoly4num);
817 for(i=0; i < clippedPoly4num; i++) {
818 debugpts.push_back(clippedPoly4[i]);
826 for(i = 0; i < clippedPoly4num; i++)
831 double disp = vecdot(&pp->clippedPoly4[i],&pp->clippedPoly4[i]);
835 if(disp < get_poly_mindisp)
837 get_poly_mindisp = disp;
838 result = pp->clippedPoly4[i];
843 if(get_poly_mindisp <= r*r)
853 rl = veclength(result);
860 vecscale(&result,&result,(r-sqrt(get_poly_mindisp)) / rl);
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)
887 ppoly[0] = project_on_yplane(p1,n,y);
888 ppoly[1] = project_on_yplane(p2,n,y);
895 for(i= 0; i < 2; i++) {
896 if(ppoly[i].x*ppoly[i].x + ppoly[i].z*ppoly[i].z > r*r) {
899 clippedpoly[clippedpolynum++] = ppoly[i];
911 nsect = getk_intersect_segment_with_ycylinder(&k1,&k2,r,ppoly[0],ppoly[1]);
914 if(fabs(k1-k2) < FLOAT_TOLERANCE)
916 clippedpoly[clippedpolynum++] = weighted_sum(ppoly[0],ppoly[1],k2);
918 clippedpoly[clippedpolynum++] = weighted_sum(ppoly[0],ppoly[1],k1);
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) {
926 clippedpoly[clippedpolynum++] = dessect;
931 return clippedpolynum;
945 int clippedpolynum = 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);
959 if(! APPROX(n.y, 1) && ! APPROX(n.y, -1)) {
967 if(intersect_segment_with_line_on_yplane(&dessect3d,p[0],p[1],n,zero)) {
968 dessect3d = project_on_cylindersurface_plane(dessect3d,n,r);
970 if(dessect3d.y < y2 &&
972 clippedpoly[clippedpolynum++] = dessect3d;
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;
982 vecscale(§[i],&n,k2);
983 VECADD(sect[i],p[i]);
985 if(sect[i].y > y1 && sect[i].y < y2) {
986 clippedpoly[clippedpolynum++] = sect[i];
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];
1002 for(i=0; i < clippedpolynum; i++) {
1003 debugpts.push_back(clippedpoly[i]);
1008 polydisp = vecdot(&p[0],&n);
1011 for(i = 0; i < clippedpolynum; i++) {
1012 double disp = vecdot(&clippedpoly[i],&n) - polydisp;
1013 if(disp < mindisp) mindisp = disp;
1015 vecscale(&result,&n,mindisp);
1025 double dmax = -1E99;
1028 int clippedPoly5num = 0;
1031 pp->get_poly_mindisp = 1E90;
1034 printf(
"facemask = %d, debugsurface = %d\n",facemask,debugsurface);
1035 if((facemask & (1 <<debugsurface++)) )
return zero;
1038 if((p1.y > y2 || p2.y > y2 || n.y < 0) && n.y < STEPUP_MAXINCLINE)
1042 if ((10)> pp->clippedPoly5Size) {
1043 pp->clippedPoly5 = (
struct point_XYZ*) REALLOC(pp->clippedPoly5,
sizeof(
struct point_XYZ) * (10));
1044 pp->clippedPoly5Size = 10;
1048 clippedPoly5num = helper_line_clip_cap(pp->clippedPoly5, clippedPoly5num, p1, p2, r, n, y1,1 );
1051 for(i=0; i < clippedPoly5num; i++) {
1052 debugpts.push_back(clippedPoly5[i]);
1057 for(i = 0; i < clippedPoly5num; i++) {
1058 if(pp->clippedPoly5[i].y > dmax)
1059 dmax = pp->clippedPoly5[i].y;
1066 pp->get_poly_mindisp = y1-dmax;
1070 result.y = pp->get_poly_mindisp;
1085 result = get_line_step_disp(y1,ystep,r,p1,p2,n);
1087 if (! APPROX(result.y, 0)) {
1090 return get_line_normal_disp(y1,y2,r,p1,p2,n);
1103 if((p1.y <= ystep && p1.y > y1 && p1.x*p1.x + p1.z*p1.z < r*r) && (n.y > STEPUP_MAXINCLINE) ) {
1109 y = (n.y < 0.) ? y2 : y1;
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);
1123 if (! APPROX(n.y, 1) && ! APPROX(n.y, -1)) {
1127 nsect = getk_intersect_line_with_ycylinder(&k1, &k2, r, p1, n);
1130 if(k2 >= 0)
return zero;
1131 vecscale(&result,&n,k2);
1135 if(cp.y > y1 && cp.y < y2) {
1155 static const int faces[6][4] = {
1165 for(ci = 0; ci < 8; ci++) p[ci] = p0;
1171 VECADD(p[4],i); VECADD(p[4],j); VECADD(p[4],k);
1172 VECADD(p[5],k); VECADD(p[5],i);
1173 VECADD(p[6],j); VECADD(p[6],k);
1174 VECADD(p[7],i); VECADD(p[7],j);
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.);
1190 for(ci = 0; ci < 6; ci++) {
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]];
1203 dispv = get_poly_disp_2(pts,4,n[ci]);
1204 disp = vecdot(&dispv,&dispv);
1210 if( (disp > FLOAT_TOLERANCE) && (disp > maxdisp) ){
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;
1226 double lefteq = sqrt(y*y + r*r) + sqrt(xs*xs + ys*ys + zs*zs);
1227 return lefteq*lefteq > vecdot(&pcenter,&pcenter);
1231double *transformFULL4d(
double *r4,
double *a4,
double *mat);
1232void __gluMultMatrixVecd(
const GLDOUBLE matrix[16],
const GLDOUBLE in[4], GLDOUBLE out[4]);
1234int transformMBB4d(GLDOUBLE *rMBBmin, GLDOUBLE *rMBBmax, GLDOUBLE *matTransform, GLDOUBLE* inMBBmin, GLDOUBLE* inMBBmax,
int isAffine)
1243 GLDOUBLE p[3],rx,ry,rz;
1249 rx = i==0? inMBBmin[0] : inMBBmax[0];
1252 ry = j==0? inMBBmin[1] : inMBBmax[1];
1255 rz = k==0? inMBBmin[2] : inMBBmax[2];
1267 transform(&abox[m],&abox[m],matTransform);
1272 pointxyz2double(in,&abox[m]);
1274 __gluMultMatrixVecd(matTransform, in, out);
1276 if(0)
if (fabs(out[3]) < .0001) {
1280 vecscaled(out,out,1.0/out[3]);
1281 double2pointxyz(&abox[m],out);
1288 pointxyz2double(rMBBmin,&abox[0]);
1289 pointxyz2double(rMBBmax,&abox[0]);
1293 pointxyz2double(p,&abox[m]);
1296 rMBBmin[i] = DOUBLE_MIN(rMBBmin[i],p[i]);
1297 rMBBmax[i] = DOUBLE_MAX(rMBBmax[i],p[i]);
1303void transformMBB(GLDOUBLE *rMBBmin, GLDOUBLE *rMBBmax, GLDOUBLE *matTransform, GLDOUBLE* inMBBmin, GLDOUBLE* inMBBmax){
1305 int isAffine = 1, iret;
1309 iret = transformMBB4d(rMBBmin, rMBBmax, matTransform, inMBBmin, inMBBmax,isAffine);
1315int fast_ycylinder_MBB_intersect_collisionSpace(
double y1,
double y2,
double r, GLDOUBLE *shape2collision, GLDOUBLE *shapeMBBmin, GLDOUBLE *shapeMBBmax )
1326 GLDOUBLE smin[3], smax[3], avmin[3], avmax[3];
1334 avmin[1] = y1; avmax[1] = y2;
1336 transformMBB(smin,smax,shape2collision,shapeMBBmin,shapeMBBmax);
1337 return overlapMBBs(avmin,avmax,smin,smax);
1341int fast_sphere_MBB_intersect_collisionSpace(
double r, GLDOUBLE *shape2collision, GLDOUBLE *shapeMBBmin, GLDOUBLE *shapeMBBmax )
1347 GLDOUBLE smin[3], smax[3], avmin[3], avmax[3];
1355 transformMBB(smin,smax,shape2collision,shapeMBBmin,shapeMBBmax);
1356 return overlapMBBs(avmin,avmax,smin,smax);
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;
1365 double lefteq = sqrt(y*y + r*r) + sqrt(halfheight*halfheight + baseradius*baseradius);
1366 return lefteq*lefteq > vecdot(&pcenter,&pcenter);
1373struct point_XYZ cone_disp(double y1, double y2, double ystep, double r, struct
point_XYZ base, struct
point_XYZ top, double baseradius) {
1385 double mindisp = 1E99;
1389 vecscale(&bn,&base,-1.0);
1391 VECDIFF(top,base,i);
1392 vecscale(&tmp,&i,- vecdot(&i,&bn)/vecdot(&i,&i));
1395 if (APPROX(vecnormal(&bn,&bn), 0)) {
1400 vecnormal(&tmpn,&tmpn);
1401 make_orthogonal_vector_space(&bn,&tmpj,tmpn);
1404 vecscale(&side,&bn,baseradius);
1408 h = vecnormal(&i,&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);
1422 if( vecdot(&normalside,&top) < 0. ) {
1423 dispv = get_line_disp(y1,y2,ystep,r,top,side,normalside);
1424 disp = vecdot(&dispv,&dispv);
1426 mindispv = dispv, mindisp = disp;
1429 if( vecdot(&normalbase,&base) < 0. ) {
1430 dispv = get_line_disp(y1,y2,ystep,r,base,side,normalbase);
1431 disp = vecdot(&dispv,&dispv);
1433 mindispv = dispv, mindisp = disp;
1436 if( vecdot(&normaltop,&top) < 0. ) {
1437 dispv = get_point_disp(y1,y2,ystep,r,top,normaltop);
1438 disp = vecdot(&dispv,&dispv);
1442 if(! APPROX(disp, 0) && disp < mindisp)
1443 mindispv = dispv, mindisp = disp;
1453struct point_XYZ cylinder_disp(double y1, double y2, double ystep, double r, struct
point_XYZ base, struct
point_XYZ top, double baseradius) {
1465 double mindisp = 1E99;
1469 vecscale(&bn,&base,-1.0);
1471 VECDIFF(top,base,i);
1472 vecscale(&tmp,&i,- vecdot(&i,&bn)/vecdot(&i,&i));
1475 if (APPROX(vecnormal(&bn,&bn), 0)) {
1480 vecnormal(&tmpn,&tmpn);
1481 make_orthogonal_vector_space(&bn,&tmpj,tmpn);
1483 vecscale(&sidebase,&bn,baseradius);
1485 VECADD(sidetop,sidebase);
1486 VECADD(sidebase,base);
1491 vecscale(&normalbase,&normaltop,-1.0);
1499 if( vecdot(&normalside,&sidetop) < 0. ) {
1500 dispv = get_line_disp(y1,y2,ystep,r,sidetop,sidebase,normalside);
1501 disp = vecdot(&dispv,&dispv);
1503 mindispv = dispv, mindisp = disp;
1506 if( vecdot(&normalbase,&base) < 0. ) {
1507 dispv = get_line_disp(y1,y2,ystep,r,base,sidebase,normalbase);
1508 disp = vecdot(&dispv,&dispv);
1510 mindispv = dispv, mindisp = disp;
1513 if( vecdot(&normaltop,&top) < 0. ) {
1514 dispv = get_line_disp(y1,y2,ystep,r,top,sidetop,normaltop);
1515 disp = vecdot(&dispv,&dispv);
1517 mindispv = dispv, mindisp = disp;
1526static int intersectionHeightOfVerticalLineWithSurfaceElement(
double* height,
struct point_XYZ* p,
int num,
struct point_XYZ* n,
double *tmin,
double *tmax )
1543 overlap = tmax[0] >= 0.0 && tmin[0] <= 0.0 && tmax[2] >= 0.0 && tmin[2] <= 0.0;
1544 if(!overlap)
return 0;
1546 dd = -vecdot(&p[0],n);
1552 ndotD = vecdot(n,&D);
1554 if( fabs(ndotD) < .1 )
1558 *height = - dd/ndotD;
1563 return pointOnPlaneInsidePoly(D,p,num,n);
1566void accumulateFallingClimbing(
double y1,
double y2,
double ystep,
struct point_XYZ *p,
int num,
struct point_XYZ nused,
double *tmin,
double *tmax)
1576 if( intersectionHeightOfVerticalLineWithSurfaceElement(&hh,&p[0],num,&nused, tmin, tmax))
1579 double hhbelowy1 = hh - y1;
1583 if( hh < y1 && hh > -fi->fallHeight)
1587 fi->hfall = hhbelowy1;
1589 if(hhbelowy1 > fi->hfall) fi->hfall = hhbelowy1;
1599 if( fi->isClimb == 0 )
1600 fi->hclimb = hhbelowy1;
1602 fi->hclimb = DOUBLE_MAX(fi->hclimb,hhbelowy1);
1617 if( hh > 0.0 && hh < y2 )
1655 for(i = 0; i < pr->ntri; i++)
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;
1661 if (ccw) frontfacing = (vecdot(&n[i],&p[0]) < 0);
1662 else frontfacing = (vecdot(&n[i],&p[0]) >= 0);
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)) )
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;
1693 vecscale(&nused,&n[i],-1.0);
1734 dispv = get_poly_disp_2(p, 3, nused);
1735 disp = vecdot(&dispv,&dispv);
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);
1748 if( (disp > FLOAT_TOLERANCE) && (disp > maxdisp) )
1756 VECADD(dispsum,maxdispv);
1761#undef POLYREP_DISP2_PERFORMANCE
1762#ifdef POLYREP_DISP2_PERFORMANCE
1764static bool timing = FALSE;
1765static double startTime = 0.0;
1766static double stopTime = 0.0;
1767static double accumTime = 0.0;
1768static int counter = 0;
1784#ifdef POLYREP_DISP2_PERFORMANCE
1786 printf (
"start timing polyrep_disp2\n");
1790 startTime = Time1970sec();
1796 if ((pr.VBO_buffers[VERTEX_VBO] != 0) && pp->OpenCL_Collision_Program_initialized) {
1798 float awidth = (float) tg->Bindable.naviinfo.width;
1801 for (i=0; i<16; i++) {
1802 mymat[i] = (float) mat[i];
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);
1812#ifdef POLYREP_DISP2_PERFORMANCE
1813 stopTime = Time1970sec();
1814 accumTime += stopTime - startTime;
1816 if (counter == 25) {
1817 printf (
"polyrep_disp2, averaged over 25 runs: %f\n",
1833 pp->res.x=0.0; pp->res.y=0.0; pp->res.z=0.0;
1836 for(i = 0; i < pr.ntri*3; i++) {
1837 if (pr.cindex[i] > maxc) {maxc = pr.cindex[i];}
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;
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);
1851 pr.actualCoord = pp->prd_newc_floats;
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;
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]);
1865 pp->res = polyrep_disp_rec2(&pr,pp->prd_normals,pp->res,flags);
1871#ifdef POLYREP_DISP2_PERFORMANCE
1872 stopTime = Time1970sec();
1873 accumTime += stopTime - startTime;
1875 if (counter == 25) {
1876 printf (
"polyrep_disp2, averaged over 25 runs: %f\n",
1898 double lmaxdisp = 0;
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;
1911 frontfacing = (vecdot(&n,&p[0]) < 0);
1913 if(!frontfacing && !(flags & PR_DOUBLESIDED))
return dispsum;
1915 if(!frontfacing) vecscale(&n,&n,-1.0);
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;
1929 dispv = get_poly_disp_2(p, 3, n);
1930 disp = -pp->get_poly_mindisp;
1933 printf(
"polyd: (%f,%f,%f) |%f|\n",dispv.x,dispv.y,dispv.z,disp);
1940 if((disp > FLOAT_TOLERANCE) && (disp > lmaxdisp)) {
1946 VECADD(dispsum,maxdispv);
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) {
1958 pp->res.x=0.0; pp->res.y=0.0; pp->res.z=0.0;
1961 for(i = 0; i < pr.ntri*3; i++) {
1962 if (pr.cindex[i] > maxc) {maxc = pr.cindex[i];}
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;
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);
1974 pr.actualCoord = pp->prd_newc_floats;
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]);
1982 pp->res = planar_polyrep_disp_rec(y1,y2,ystep,r,&pr,n,pp->res,flags);
1992static void get_collisionoffset(
double *x,
double *y,
double *z)
2000 ci = CollisionInfo();
2002 naviinfo = (
struct sNaviInfo*)tg->Bindable.naviinfo;
2008 xyz.x = xyz.y = xyz.z = 0.0;
2010 if(ci->Count > 0 && !APPROX(vecnormal(&res, &res),0.0) )
2011 vecscale(&xyz, &res, sqrt(ci->Maximum2));
2016 if(fi->canFall && fi->isFall )
2019 double floatfactor = .1;
2020 if(fi->allowClimbing) floatfactor = 0.0;
2023 double fallstep = DOUBLE_MIN(fi->hfall,fi->hfall * 2.0 * (TickTime() - lastTime()));
2025 xyz.y = DOUBLE_MAX(fi->hfall,fallstep);
2027 xyz.y = fi->hfall + naviinfo->height*floatfactor;
2030 if(fi->isClimb && fi->allowClimbing)
2035 if(fi->smoothStep && FALSE){
2036 double fallstep = DOUBLE_MIN(fi->hclimb, fi->hclimb * 8.0 * (TickTime() - lastTime()));
2038 xyz.y = DOUBLE_MIN(fi->hclimb,fallstep);
2046 xyz = fi->pencorrection;
2050 transform3x3(&xyz,&xyz,fi->collision2avatar);
2058void render_collisions(
int Viewer_type) {
2063 naviinfo = (
struct sNaviInfo*)gglobal()->Bindable.naviinfo;
2065 if(!(Viewer_type == VIEWER_WALK || Viewer_type == VIEWER_FLY))
return;
2066 ci = CollisionInfo();
2081 fi->fallHeight = 100.0*naviinfo->height;
2082 fi->climbHeight = 100.0*naviinfo->height;
2084 fi->walking = Viewer_type == VIEWER_WALK;
2085 fi->canFall = fi->walking;
2090 fi->allowClimbing = 1;
2092 fi->canPenetrate = 1;
2093 fi->isPenetrate = 0;
2101 avatar2BoundViewpointVerticalAvatar(fi->avatar2collision, fi->collision2avatar);
2106 loadIdentityMatrix(fi->avatar2collision);
2107 loadIdentityMatrix(fi->collision2avatar);
2111 if(fi->walking && fi->canPenetrate)
2116 lastpos = viewer_lastP_get();
2117 transform(&lastpos,&lastpos,fi->avatar2collision);
2119 plen = sqrt(vecdot(&lastpos,&lastpos));
2120 if(APPROX(plen,0.0))
2121 fi->canPenetrate = 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;
2139 render_hier(rootNode(), VF_Collision);
2140 if(!fi->isPenetrate)
2145 viewer_lastP_clear();
2147 get_collisionoffset(&(v.x), &(v.y), &(v.z));
2158#ifdef DEBUG_SCENE_EXPORT
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];
2169 printf(
"%d};\n",pr.cindex[i]);
2170 if(pr.cindex[i] > npoints)
2171 npoints = pr.cindex[i];
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]);
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");
2186void printmatrix(GLDOUBLE* mat) {
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" :
"");