basis::basis(const vec &p, const vec &c, const String& pname) {
pvecerror("basis::basis(vec &p, vec &c, char pname[12])");
name = pname;
vec dex(1,0,0);
vec dey(0,1,0);
vec dez(0,0,1);
if (length(p) == 0 || length(c) == 0) {
vecerror = 1;
ex = dex; ey = dey; ez = dez;
}
vfloat ca = cos2vec(p,c);
if (ca == 1) {
vecerror = 1;
ex = dex; ey = dey; ez = dez;
} else if (ca == -1) {
vecerror = 1;
ex = dex; ey = dey; ez = dez;
} else {
ez = unit_vec(p);
ey = unit_vec(ez || c);
ex = ey || ez;
}
}
int splane::check_point_inside1(const point& fpt, int s_ext,
vfloat fprec) const {
if (pn.check_point_in(fpt, fprec) == 1) {
if (s_ext == 1) return 0;
return 1;
}
vec v = fpt - pn.Gpiv();
if (cos2vec(dir_ins, v) > 0) return 1;
return 0;
}
int splane::check_point_inside(const point& fpt, const vec& dir,
vfloat fprec) const {
mfunname("int splane::check_point_inside(const point& fpt, const vec& dir, "
"vfloat fprec)");
if (dir == dv0) {
// this is not useful
if (fpt == pn.Gpiv()) return 1;
vec v = fpt - pn.Gpiv();
if (cos2vec(dir_ins, v) >= -vprecision) return 1;
return 0;
}
if (pn.check_point_in(fpt, fprec) == 1) {
vfloat ca = cos2vec(dir, dir_ins);
if (ca < 0) return 0;
return 1;
}
vec v = fpt - pn.Gpiv();
if (cos2vec(dir_ins, v) >= 0) return 1;
return 0;
}
vfloat ang2vec(const vec& r1, const vec& r2) {
// angle between vectors
// instead of return acos(cos2vec(r1,r2)); which produces NaN on linux at
// parallel vectors
vfloat cs = cos2vec(r1, r2);
if (vecerror != 0) return 0;
if (cs > 0.707106781187 || cs < -0.707106781187) { // 1.0/sqrt(2)
// pass to sin, it will be more exactly
vfloat sn = sin2vec(r1, r2);
if (vecerror != 0) return 0;
if (cs > 0.0) return asin(sn);
else return M_PI - asin(sn);
}
return acos(cs);
}
basis::basis(const vec &p, const String& pname) {
pvecerror("basis::basis(vec &p)");
name = pname;
//strcpy(name,pname);
vec dex(1,0,0);
vec dey(0,1,0);
vec dez(0,0,1);
if (length(p) == 0) {
vecerror = 1;
ex = dex; ey = dey; ez = dez;
}
vfloat ca = cos2vec(p,dez);
if (ca == 1) {
ex = dex; ey = dey; ez = dez;
} else if (ca == -1) {
ex = -dex; ey = -dey; ez = -dez;
} else {
ez = unit_vec(p);
ey = unit_vec(ez || dez);
ex = ey || ez;
}
}
int splane::range(const trajestep& fts, vfloat* crange, point* cpt,
int* s_ext) const {
mfunname("int splane::range(...)");
if (fts.s_range_cf == 0) {
// straight line
point pt = pn.cross(straight(fts.currpos, fts.dir));
if (vecerror != 0) {
vecerror = 0;
return 0;
}
vfloat rng = length(pt - fts.currpos);
if (pt == fts.currpos || check_par(pt - fts.currpos, fts.dir, 0.01) == 1) {
// looks like not matter ^
// otherwise the point is behind plane
if (fts.mrange >= rng) {
// otherwise it can not reach the plane
cpt[0] = pt;
crange[0] = rng;
vfloat t = cos2vec(fts.dir, dir_ins);
if (t < 0)
s_ext[0] = 1;
else if (t > 0)
s_ext[0] = 0;
else
s_ext[0] = 2;
return 1;
}
return 0;
} else
return 0;
} else {
point pt[2];
circumf cf(fts.currpos + fts.relcen,
fts.dir || fts.relcen, // if to us, moving against clock
length(fts.relcen));
int q = cf.cross(pn, pt, 0.0);
if (q == -1) // total circle lyes in the plane
{
cpt[0] = fts.currpos;
crange[0] = 0.0;
s_ext[0] = 2;
return 1;
}
if (q == 0) return 0;
if (q == 1) {
vec r1 = -fts.relcen;
vec r2 = pt[0] - cf.Gpiv();
vfloat angle = ang2projvec(r1, r2, cf.Gdir());
vfloat rng = cf.Grad() * angle;
if (fts.mrange >= rng) {
cpt[0] = pt[0];
crange[0] = rng;
vfloat c = cos2vec(dir_ins, fts.relcen);
if (angle == 0.0) {
// cross in the current point
if (c > 0)
s_ext[0] = 0;
else if (c < 0)
s_ext[0] = 1;
else
s_ext[0] = 2;
} else {
if (c > 0)
s_ext[0] = 1;
else if (c < 0)
s_ext[0] = 0;
else
s_ext[0] = 2;
}
return 1;
} else
return 0;
}
if (q == 2) {
int qq = 0;
vec r = -fts.relcen;
vec vcr[2];
vcr[0] = pt[0] - cf.Gpiv();
vcr[1] = pt[1] - cf.Gpiv();
vfloat angle[2];
angle[0] = ang2projvec(r, vcr[0], cf.Gdir());
angle[1] = ang2projvec(r, vcr[1], cf.Gdir());
if (angle[0] > angle[1]) { // ordering
vfloat a = angle[0];
angle[0] = angle[1];
angle[1] = a;
point p = pt[0];
pt[0] = pt[1];
pt[1] = p;
}
vfloat rng;
rng = cf.Grad() * angle[0];
if (fts.mrange >= rng) {
// find out what the first point means
int ins = 0; // 1 if the point inside and exits
vec td = fts.dir;
td.turn(cf.Gdir(), angle[0]); // local dir in the crossing point
vfloat t = cos2vec(td, dir_ins);
if (t < 0)
ins = 1; // means the point was inside and now exiting
else
ins = 0;
cpt[0] = pt[0];
crange[0] = rng;
s_ext[0] = ins;
qq++;
rng = cf.Grad() * angle[1];
if (fts.mrange >= rng) {
cpt[1] = pt[1];
crange[1] = rng;
s_ext[1] = (ins == 0 ? 1 : 0);
qq++;
}
}
return qq;
}
}
return 0;
}