rendered paste body#include <adolc/adolc.h>#include <cmath>#include <map>#include <iostream>#include <typeinfo>using namespace std;const double pi = atan(1.0)*4;template<class Real>class vector3{ // three dimensional vectorpublic: Real x, y, z; vector3(): x(0), y(0), z(0){} vector3(Real x, Real y, Real z): x(x), y(y), z(z){} vector3(Real THETA, Real PHI): x(sin(THETA)*cos(PHI)), y(sin(THETA)*sin(PHI)), z(cos(THETA)){} template<class R> vector3(const vector3<R>& v): x(v.x), y(v.y), z(v.z){} // orthogonal unit vectors: direction of x- and y-axis, when line points z-axis direction vector3<Real> alpha(){ // orthogonal vector 1 if(norm2()==0) throw "tried to take unit vector for zero vector"; Real PHI = atan2(y,x); Real THETA = atan2(x*cos(PHI) + y*sin(PHI), z); return vector3(THETA + pi/2, PHI); } vector3<Real> beta(){ // orthogonal vector 2 if(norm2()==0) throw "tried to take unit vector for zero vector"; Real PHI = atan2(y,x); return vector3(pi/2, PHI + pi/2); } // vector calculations Real dot(const vector3<Real> v) const{ return x * v.x + y * v.y + z * v.z; } Real norm2() const{ return (x*x + y*y + z*z); } vector3 cross(const vector3<Real> v) const{ return vector3(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x); } vector3<Real> operator +(const vector3<Real> v) const{ return vector3<Real>(x+v.x, y+v.y, z+v.z); } vector3<Real> operator -(const vector3<Real> v) const{ return vector3<Real>(x-v.x, y-v.y, z-v.z); } vector3<Real> operator *(const Real s) const{ return vector3<Real>(x*s, y*s, z*s); }};template<class Real>ostream& operator<<(ostream& os, const vector3<Real>& v){ return ( os << "vector3(" << v.x << ", " << v.y << ", " << v.z << ")" );}template<class Real>class line3{ // a line in three dimensional space. // this is a line zenith angle THETA and azimuth angle PHI that go through origin, // translated into direction of alpha and beta (corresponding to x- and y-axis if line is z-axis) by (alpha, beta).public: line3(Real THETA, Real PHI, Real alpha, Real beta): THETA(THETA), PHI(PHI), alpha(alpha), beta(beta){} line3(vector3<Real> p, vector3<Real> v){ vector3<Real> u = v.unit(); THETA = u.THETA; PHI = u.PHI; alpha = (u.cross(p)).dot(u.beta()); beta = -(u.cross(p)).dot(u.alpha()); } template<class R> line3(const line3<R>& l): THETA(l.THETA),PHI(l.PHI),alpha(l.alpha),beta(l.beta){} Real THETA; Real PHI; Real alpha; Real beta; vector3<Real> foot() const{ // return foot of perpendicular line from origin to the line vector3<Real> u(THETA, PHI); return ((vector3<Real>(u.alpha()))*alpha) + ((vector3<Real>(u.beta()))*beta); } vector3<Real> unit() const{ // return orientation of the l return vector3<Real>(THETA, PHI); }};template<class Real>class parallelogram{ // parallelogram abc(d) // b--c // / / // a--dpublic: vector3<Real> a, b, c; parallelogram(vector3<Real> a, vector3<Real> b, vector3<Real> c): a(a), b(b), c(c){} template<class R> parallelogram(parallelogram<R> p): a(p.a), b(p.b), c(p.c){} parallelogram(){} template<class R> // Real maybe const, then R is not const bool penetratedp(const line3<Real> line, vector3<R>* result) const{ // return whether line crosses this. // if that is case, store the crossing point into result. vector3<Real> X = line.foot(), u = line.unit(), delta1 = b-a, delta2=b-c; pair<R, R> r1, r2; // range // t such that X0 + t u crosses plane abc R t_on;#ifdef DEBUG cout << " penetratedp()" << endl; cout << " a,b,c:" << a << b << c << endl; cout << " X:" << X << endl; cout << " u:" << u << endl;#endif if(line_on_plane(X, u, &t_on) && range_in(X, u, b, c, delta1, &r1) && range_in(X, u, b, a, delta2, &r2)){#ifdef DEBUG cout << " a,b,c:" << a << b << c << endl; cout << " r1:" << r1.first << ", " << r1.second << endl; cout << " r2:" << r2.first << ", " << r2.second << endl; cout << " t_on:" << t_on << endl;#endif if( (r1.first <= t_on && t_on <= r1.second) && (r2.first <= t_on && t_on <= r2.second) ){ if(result != NULL){ *result = X + u * t_on; } return true; }else{ return false; } }else{ return false; } } template<class R> bool line_on_plane(const vector3<Real> X, const vector3<Real> u, R* result) const{ // return wheather a line (X + t u) crosses plane abc. if that is // case, substitute the point into result vector3<Real> n = (b-a).cross(c-b); if(n.dot(u) == 0){ // won't cross at one point (we ignore overwrapping case) return false; }else{ *result = n.dot(a-X) / n.dot(u); return true; } } template<class R> bool range_in(const vector3<Real> X, const vector3<Real> u, const vector3<Real> p, const vector3<Real> q, const vector3<Real> delta, pair<R, R>* result) const{ // return whether the line (X+tu) will be between two parallel lines (p+s*delta), (q+s*delta). // if so, store the range of t where the point (X+tu) sees two lines in opposite direction, into result.#ifdef DEBUG cout << " range_in()" << endl; cout << " before: a,b,c:" << a << b << c << endl; cout << " X, u:" << X << u << endl; cout << " calculating A.." << endl;#endif Real A = u.cross(delta).norm2();#ifdef DEBUG cout << " X, u:" << X << u << endl; cout << " calculating B.." << endl;#endif Real B = u.cross(delta).dot((X*2-p-q).cross(delta));#ifdef DEBUG cout << " X, u:" << X << u << endl; cout << " calculating C.." << endl;#endif Real C = ((X-p).cross(delta)).dot((X-q).cross(delta));#ifdef DEBUG cout << " X, u:" << X << u << endl; cout << " calculating D.." << endl;#endif Real D = B*B - 4 * A*C;#ifdef DEBUG cout << " X, u:" << X << u << endl;#endif#ifdef DEBUG cout << " after: a,b,c:" << a << b << c << endl; cout << " A==" << A << ",B==" << B << ",C==" << C << ",D==" << D << endl;#endif if(D<0){ return false; }else{ *result = make_pair((-B-sqrt(D))/(2*A), (-B+sqrt(D))/(2*A)); return true; } }};#define TEST_ROUTINE { \ cout << "checking for " << typeid (D).name() << endl; \ vector3<const D> a(0,0,0), b(1,0,0), c(1,1,0); \ parallelogram<const D> p(a,b,c); \ vector3<D> tmp; \ cout << "result:"<< (p.penetratedp(line3<const double>(0,0,0.5,0.5), &tmp) ? "yes": "no") << endl;}int main(){ { #define D double TEST_ROUTINE }#undef D { #define D adouble TEST_ROUTINE } return 0;}