/******************************************************************************/ #include "stdafx.h" namespace EE{ /******************************************************************************/ #define SCALE 1.5f // max assumed stretch (for example if the original box is set to a character in rest pose, but due to animation it moves its arms around, then the box after animation might be bigger than original non-animated box, this allows to specify max allowed scale of the original box) /****************************************************************************** Ball& Ball::setAnimated(C Box &box, C Matrix &matrix) { r =box.size ().length()*matrix.x.length()*(0.5f*SCALE); pos=box.center() *matrix; return T; } Ball& Ball::setAnimated(C Box &box, C AnimatedSkeleton &anim_skel) { r =box.size ().length()*anim_skel.scale ()*(0.5f*SCALE); pos=box.center() *anim_skel.matrix(); return T; } /******************************************************************************/ Ball& Ball::setAnimated(C Extent &ext, C Matrix &matrix) { r =ext.ext.length()*matrix.x.length()*SCALE; pos=ext.pos *matrix; return T; } Ball& Ball::setAnimated(C Extent &ext, C AnimatedSkeleton &anim_skel) { r =ext.ext.length()*anim_skel.scale ()*SCALE; pos=ext.pos *anim_skel.matrix(); return T; } /******************************************************************************/ Ball::Ball(C Extent &ext ) {set( ext.ext .length() , ext .pos );} Ball::Ball(C Box &box ) {set( box.size().length()*0.5f, box .center());} Ball::Ball(C OBox &obox ) {set(obox.box.size().length()*0.5f, obox .center());} Ball::Ball(C Capsule &capsule) {set( capsule.h*0.5f, capsule.pos );} Ball::Ball(C Shape &shape ) { switch(shape.type) { case SHAPE_BOX : T=shape.box ; break; case SHAPE_OBOX : T=shape.obox ; break; case SHAPE_BALL : T=shape.ball ; break; case SHAPE_CAPSULE: T=shape.capsule; break; } } Ball::Ball(C MeshBase &mesh) {from(mesh.vtx.pos(), mesh.vtxs());} Ball& Ball::operator*=( Flt f) {pos*=f; r*=f; return T;} Ball& Ball::operator/=( Flt f) {pos/=f; r/=f; return T;} Ball& Ball::operator*=(C Matrix3 &m) { pos*=m; r *=m.maxScale(); return T; } Ball& Ball::operator*=(C Matrix &m) { pos*=m; r *=m.maxScale(); return T; } Ball& Ball::operator/=(C Matrix3 &m) { pos/=m; r /=m.maxScale(); return T; } Ball& Ball::operator/=(C Matrix &m) { pos/=m; r /=m.maxScale(); return T; } /******************************************************************************/ Bool Ball::from(C Vec *point, Int points) { Box box; if(box.from(point, points)) { pos=box.center(); Flt dist2=0; REP(points)MAX(dist2, Dist2(point[i], pos)); r=SqrtFast(dist2); return true; } zero(); return false; } Bool Ball::from(C Vec *point, Int points, C Matrix &matrix) { Box box; if(box.from(point, points)) { pos=box.center()*matrix; Flt dist2=0; REP(points)MAX(dist2, Dist2(point[i]*matrix, pos)); r=SqrtFast(dist2); return true; } zero(); return false; } /******************************************************************************/ Vec Ball::nearest(C Vec &normal)C {return pos-normal*r;} /******************************************************************************/ void Ball::drawVI(Bool fill, C VecI2 &resolution)C { Int res_x=((resolution.x<0) ? 24 : ((resolution.x<3) ? 3 : resolution.x)), res_y=((resolution.y<0) ? 24 : ((resolution.y<3) ? 3 : resolution.y)); Flt y1_sr=r, y1_cr=0; REPD(y, res_y-1) { Flt y2_cr, y2_sr; CosSin(y2_cr, y2_sr, -PI_2+PI*y/(res_y-1)); y2_sr*=r; y2_cr*=r; Flt x1_s=0, x1_c=1; REPD(x, res_x) { Flt x2_c, x2_s; CosSin(x2_c, x2_s, (PI2*x)/res_x); if(fill) { VI.quad(pos+Vec(x2_c*y1_cr, y1_sr, x2_s*y1_cr), pos+Vec(x1_c*y1_cr, y1_sr, x1_s*y1_cr), pos+Vec(x1_c*y2_cr, y2_sr, x1_s*y2_cr), pos+Vec(x2_c*y2_cr, y2_sr, x2_s*y2_cr)); }else { VI.line(pos+Vec(x1_c*y1_cr, y1_sr, x1_s*y1_cr), pos+Vec(x1_c*y2_cr, y2_sr, x1_s*y2_cr)); if(y)VI.line(pos+Vec(x1_c*y2_cr, y2_sr, x1_s*y2_cr), pos+Vec(x2_c*y2_cr, y2_sr, x2_s*y2_cr)); } x1_s=x2_s; x1_c=x2_c; } y1_sr=y2_sr; y1_cr=y2_cr; } } void Ball::draw(C Color &color, Bool fill, C VecI2 &resolution)C { VI.color(color); drawVI(fill, resolution); VI.end ( ); } void Ball::drawAngle(C Color &color, Flt from, Flt to, C Vec &up, Bool fill, C VecI2 &resolution)C { to-=from; Matrix m; m.setPosUp(VecZero, up).scale(r).move(pos); Int res_x=((resolution.x<0) ? 24 : ((resolution.x<3) ? 3 : resolution.x)), res_y=((resolution.y<0) ? 24 : ((resolution.y<3) ? 3 : resolution.y)); Flt y1_cr, y1_sr; CosSin(y1_cr, y1_sr, from+to); VI.color(color); REPD(y, res_y-1) { Flt y2_cr, y2_sr; CosSin(y2_cr, y2_sr, from+to*y/(res_y-1)); Flt x1_s=0, x1_c=1; REPD(x, res_x) { Flt x2_c, x2_s; CosSin(x2_c, x2_s, (PI2*x)/res_x); if(fill) { VI.quad(Vec(x2_c*y1_cr, y1_sr, x2_s*y1_cr)*m, Vec(x1_c*y1_cr, y1_sr, x1_s*y1_cr)*m, Vec(x1_c*y2_cr, y2_sr, x1_s*y2_cr)*m, Vec(x2_c*y2_cr, y2_sr, x2_s*y2_cr)*m); }else { VI.line(Vec(x1_c*y1_cr, y1_sr, x1_s*y1_cr)*m, Vec(x1_c*y2_cr, y2_sr, x1_s*y2_cr)*m); if(y)VI.line(Vec(x1_c*y2_cr, y2_sr, x1_s*y2_cr)*m, Vec(x2_c*y2_cr, y2_sr, x2_s*y2_cr)*m); } x1_s=x2_s; x1_c=x2_c; } y1_sr=y2_sr; y1_cr=y2_cr; } VI.end(); } void Ball::draw2(C Color &color, Bool fill, Int resolution)C { Matrix m[6]; REP(6)m[i].setPosOrient(pos, DIR_ENUM(i)); m[DIR_BACK].y.chs(); m[DIR_LEFT].y.chs(); m[DIR_DOWN].x.set(0, 0,-1); m[DIR_DOWN].y.set(1, 0, 0); m[DIR_DOWN].z.set(0,-1, 0); if(resolution<0)resolution=12;else if(resolution<1)resolution=1; Vec t(-resolution/2.0f, -resolution/2.0f, resolution/2.0f); VI.color(color); REPD(y, resolution) REPD(x, resolution) { Vec bp[4]= { Vec(x , y+1, 0), Vec(x+1, y+1, 0), Vec(x+1, y , 0), Vec(x , y , 0), }; REP(4){bp[i]+=t; bp[i].setLength(r);} REPD(s, 6) { Vec p[4]; Copy(p, bp); Transform(p, m[s], 4); if(fill)VI.quad(p[0], p[1], p[2], p[3]);else { VI.line(p[0], p[1]); VI.line(p[0], p[3]); } } } VI.end(); } /******************************************************************************/ Ball Avg(C Ball &a, C Ball &b) {return Ball(Avg(a.r, b.r), Avg(a.pos, b.pos));} /******************************************************************************/ Flt Dist(C Vec &point, C Ball &ball) { return Max(0, Dist(point, ball.pos)-ball.r); } Flt Dist(C Edge &edge, C Ball &ball) { return Max(0, Dist(ball.pos, edge)-ball.r); } Flt Dist(C Tri &tri, C Ball &ball) { return Max(0, Dist(ball.pos, tri)-ball.r); } Flt Dist(C Box &box, C Ball &ball) { Flt dist2=0; if(ball.pos.xbox.max.x)dist2+=Sqr(ball.pos.x-box.max.x); if(ball.pos.ybox.max.y)dist2+=Sqr(ball.pos.y-box.max.y); if(ball.pos.zbox.max.z)dist2+=Sqr(ball.pos.z-box.max.z); return Max(0, SqrtFast(dist2)-ball.r); } Flt Dist(C OBox &obox, C Ball &ball) { Ball temp=ball; temp.pos.divNormalized(obox.matrix); return Dist(obox.box, temp); } Flt Dist(C Ball &a, C Ball &b) { return Max(0, Dist(a.pos, b.pos)-a.r-b.r); } Flt DistBallPlane(C Ball &ball, C Vec &plane, C Vec &normal) { return DistPointPlane(ball.pos, plane, normal)-ball.r; } Dbl DistBallPlane(C Ball &ball, C VecD &plane, C Vec &normal) { return DistPointPlane(ball.pos, plane, normal)-ball.r; } Dbl DistBallPlane(C BallM &ball, C VecD &plane, C Vec &normal) { return DistPointPlane(ball.pos, plane, normal)-ball.r; } /******************************************************************************/ Bool Cuts(C Vec &point, C Ball &ball) { return Dist2(point, ball.pos)<=Sqr(ball.r); } Bool Cuts(C VecD &point, C Ball &ball) { return Dist2(point, ball.pos)<=Sqr(ball.r); } Bool Cuts(C VecD &point, C BallM &ball) { return Dist2(point, ball.pos)<=Sqr(ball.r); } Bool Cuts(C Edge &edge, C Ball &ball) { return Dist(ball.pos, edge)<=ball.r; } Bool Cuts(C Tri &tri, C Ball &ball) { return Dist(ball.pos, tri)<=ball.r; } Bool Cuts(C Ball &a, C Ball &b) { return Dist2(a.pos, b.pos)<=Sqr(a.r+b.r); } Bool Cuts(C Box &box, C Ball &ball) { #if 1 // fastest if(ball.pos.xbox.max.x+ball.r)return false; if(ball.pos.ybox.max.y+ball.r)return false; if(ball.pos.zbox.max.z+ball.r)return false; Flt dist2=0; if(ball.pos.xbox.max.x)dist2 =Sqr(ball.pos.x-box.max.x); if(ball.pos.ybox.max.y)dist2+=Sqr(ball.pos.y-box.max.y); if(ball.pos.zbox.max.z)dist2+=Sqr(ball.pos.z-box.max.z); return dist2<=Sqr(ball.r); #elif 1 // medium Flt dist2=0, r2=Sqr(ball.r); if(ball.pos.xr2)return false;}else if(ball.pos.x>box.max.x){dist2 =Sqr(ball.pos.x-box.max.x); if(dist2>r2)return false;} if(ball.pos.yr2)return false;}else if(ball.pos.y>box.max.y){dist2+=Sqr(ball.pos.y-box.max.y); if(dist2>r2)return false;} if(ball.pos.zr2)return false;}else if(ball.pos.z>box.max.z){dist2+=Sqr(ball.pos.z-box.max.z); if(dist2>r2)return false;} return true; #else // slowest Flt dist2=0, d; if(ball.pos.xball.r)return false; dist2 =Sqr(d);}else if(ball.pos.x>box.max.x){d=ball.pos.x-box.max.x; if(d>ball.r)return false; dist2 =Sqr(d);} if(ball.pos.yball.r)return false; dist2+=Sqr(d);}else if(ball.pos.y>box.max.y){d=ball.pos.y-box.max.y; if(d>ball.r)return false; dist2+=Sqr(d);} if(ball.pos.zball.r)return false; dist2+=Sqr(d);}else if(ball.pos.z>box.max.z){d=ball.pos.z-box.max.z; if(d>ball.r)return false; dist2+=Sqr(d);} return dist2<=Sqr(ball.r); #endif } Bool Cuts(C Extent &ext, C Ball &ball) { #if 1 // faster return Dist2(Max(0, Abs(ext.pos.x-ball.pos.x)-ext.ext.x), Max(0, Abs(ext.pos.y-ball.pos.y)-ext.ext.y), Max(0, Abs(ext.pos.z-ball.pos.z)-ext.ext.z))<=Sqr(ball.r); #else // slower Flt dist2=0, d; d=Abs(ext.pos.x-ball.pos.x)-ext.ext.x; if(d>0)dist2 =Sqr(d); d=Abs(ext.pos.y-ball.pos.y)-ext.ext.y; if(d>0)dist2+=Sqr(d); d=Abs(ext.pos.z-ball.pos.z)-ext.ext.z; if(d>0)dist2+=Sqr(d); return dist2<=Sqr(ball.r); #endif } Bool Cuts(C OBox &obox, C Ball &ball) { Ball temp; // 'ball' in 'obox' space temp.r=ball.r; temp.pos.fromDivNormalized(ball.pos, obox.matrix); return Cuts(obox.box, temp); } /******************************************************************************/ Int CutsStrBall(C Vec &str_pos, C Vec &str_dir, C Ball &ball, Vec *contact_a, Vec *contact_b) { Vec p=PointOnPlane(str_pos, ball.pos, str_dir); Flt s=Dist (p , ball.pos )/ball.r; if(s> 1)return 0; if(s==1){if(contact_a)*contact_a=p; return 1;} if(contact_a || contact_b) { s=Sqrt(1-s*s)*ball.r; if(contact_a)*contact_a=p-s*str_dir; if(contact_b)*contact_b=p+s*str_dir; } return 2; } Int CutsEdgeBall(C Vec &edge_start, C Vec &edge_end, C Ball &ball, Vec *contact_a, Vec *contact_b) { Vec c[2], dir=!(edge_end-edge_start); if(Int hit=CutsStrBall(edge_start, dir, ball, &c[0], &c[1])) { Bool in[2]={(hit>=1), (hit>=2)}; REP(2)if(in[i])if(DistPointPlane(c[i], edge_start, dir)<0 || DistPointPlane(c[i], edge_end, dir)>0)in[i]=false; if(in[0] && in[1]){if(contact_a)*contact_a=c[ 0]; if(contact_b)*contact_b=c[1]; return 2;} if(in[0] || in[1]){if(contact_a)*contact_a=c[in[1]]; return 1;} } return 0; } /******************************************************************************/ Bool Inside(C Box &a, C Ball &b) { #if 0 // this is slower return Dist2(Max(Abs(a.max.x-b.pos.x), Abs(a.min.x-b.pos.x)), Max(Abs(a.max.y-b.pos.y), Abs(a.min.y-b.pos.y)), Max(Abs(a.max.z-b.pos.z), Abs(a.min.z-b.pos.z)))<=Sqr(b.r); #else // this is faster return Max(Sqr(a.max.x-b.pos.x), Sqr(a.min.x-b.pos.x)) +Max(Sqr(a.max.y-b.pos.y), Sqr(a.min.y-b.pos.y)) +Max(Sqr(a.max.z-b.pos.z), Sqr(a.min.z-b.pos.z))<=Sqr(b.r); #endif } Bool Inside(C Extent &a, C Ball &b) { return Dist2(((a.pos.x>b.pos.x) ? a.maxX() : a.minX())-b.pos.x, ((a.pos.y>b.pos.y) ? a.maxY() : a.minY())-b.pos.y, ((a.pos.z>b.pos.z) ? a.maxZ() : a.minZ())-b.pos.z)<=Sqr(b.r); } /******************************************************************************/ Bool SweepPointBall(C Vec &point, C Vec &move, C Ball &ball, Flt *hit_frac, Vec *hit_normal) { Vec dir =move; Flt length=dir.normalize(); Vec p =PointOnPlane(point, ball.pos, dir); Flt s =Dist(p, ball.pos)/ball.r; if(s>1)return false; p -=dir*(CosSin(s)*ball.r); Flt dist=DistPointPlane(p, point, dir); if(dist<0 || dist>length)return false; if(hit_frac )*hit_frac =dist/length; if(hit_normal)*hit_normal=(p-ball.pos)/ball.r; return true; } Bool SweepPointBall(C VecD &point, C VecD &move, C BallD &ball, Dbl *hit_frac, VecD *hit_normal) { VecD dir =move; Dbl length=dir.normalize(); VecD p =PointOnPlane(point, ball.pos, dir); Dbl s =Dist(p, ball.pos)/ball.r; if(s>1)return false; p -=dir*(CosSin(s)*ball.r); Dbl dist=DistPointPlane(p, point, dir); if(dist<0 || dist>length)return false; if(hit_frac )*hit_frac =dist/length; if(hit_normal)*hit_normal=(p-ball.pos)/ball.r; return true; } /******************************************************************************/ Bool SweepBallPoint(C Ball &ball, C Vec &move, C Vec &point, Flt *hit_frac, Vec *hit_normal) { Vec dir =move; Flt length=dir.normalize(); Vec p =PointOnPlane(ball.pos, point, dir); Flt s =Dist(p, point)/ball.r; if(s>1)return false; p -=dir*(CosSin(s)*ball.r); Flt dist=DistPointPlane(p, ball.pos, dir); if(dist<0 || dist>length)return false; if(hit_frac )*hit_frac =dist/length; if(hit_normal)*hit_normal=(p-point)/ball.r; return true; } Bool SweepBallPoint(C BallD &ball, C VecD &move, C VecD &point, Dbl *hit_frac, VecD *hit_normal) { VecD dir =move; Dbl length=dir.normalize(); VecD p =PointOnPlane(ball.pos, point, dir); Dbl s =Dist(p, point)/ball.r; if(s>1)return false; p -=dir*(CosSin(s)*ball.r); Dbl dist=DistPointPlane(p, ball.pos, dir); if(dist<0 || dist>length)return false; if(hit_frac )*hit_frac =dist/length; if(hit_normal)*hit_normal=(p-point)/ball.r; return true; } /******************************************************************************/ Bool SweepBallEdge(C Ball &ball, C Vec &move, C Edge &edge, Flt *hit_frac, Vec *hit_normal) // safe in case 'edge' is zero length { Int point_test; Vec dir=edge.delta(); if(dir.normalize()) // check if 'edge' has length { Matrix matrix; matrix. setPosDir(edge.p[0], dir ); Circle circle(ball.r, matrix. convert(ball.pos , true)); Vec2 move2 = matrix.orn().convert(move , true) ; Vec2 normal; Flt frac; if(SweepCirclePoint(circle, move2, Vec2(0), &frac, &normal)) { Vec point=ball.pos+frac*move; if(DistPointPlane(point, edge.p[0], matrix.z)<0)point_test=0;else if(DistPointPlane(point, edge.p[1], matrix.z)>0)point_test=1;else { if(hit_frac )*hit_frac =frac; if(hit_normal)*hit_normal=matrix.orn().convert(normal); return true; } }else point_test=Closer(ball.pos, edge.p[0], edge.p[1]); }else point_test=0; // if 'edge' is zero length then check first point return SweepBallPoint(ball, move, edge.p[point_test], hit_frac, hit_normal); } Bool SweepBallEdge(C BallD &ball, C VecD &move, C EdgeD &edge, Dbl *hit_frac, VecD *hit_normal) // safe in case 'edge' is zero length { Int point_test; VecD dir=edge.delta(); if(dir.normalize()) // check if 'edge' has length { MatrixD matrix; matrix. setPosDir(edge.p[0], dir ); CircleD circle(ball.r, matrix. convert(ball.pos , true)); VecD2 move2 = matrix.orn().convert(move , true) ; VecD2 normal; Dbl frac; if(SweepCirclePoint(circle, move2, VecD2(0), &frac, &normal)) { VecD point=ball.pos+frac*move; if(DistPointPlane(point, edge.p[0], matrix.z)<0)point_test=0;else if(DistPointPlane(point, edge.p[1], matrix.z)>0)point_test=1;else { if(hit_frac )*hit_frac =frac; if(hit_normal)*hit_normal=matrix.orn().convert(normal); return true; } }else point_test=Closer(ball.pos, edge.p[0], edge.p[1]); }else point_test=0; // if 'edge' is zero length then check first point return SweepBallPoint(ball, move, edge.p[point_test], hit_frac, hit_normal); } /******************************************************************************/ Bool SweepBallBall(C Ball &ball, C Vec &move, C Ball &ball2, Flt *hit_frac, Vec *hit_normal) { return SweepPointBall(ball.pos, move, Ball(ball.r+ball2.r, ball2.pos), hit_frac, hit_normal); // 'hit_normal' is OK when merging 2 balls, this was tested, it will always be in the line of ball positions when they are in contact ('ball' 'move'd by 'hit_frac') } Bool SweepBallBall(C BallD &ball, C VecD &move, C BallD &ball2, Dbl *hit_frac, VecD *hit_normal) { return SweepPointBall(ball.pos, move, BallD(ball.r+ball2.r, ball2.pos), hit_frac, hit_normal); // 'hit_normal' is OK when merging 2 balls, this was tested, it will always be in the line of ball positions when they are in contact ('ball' 'move'd by 'hit_frac') } /******************************************************************************/ } /******************************************************************************/