Index: cpp/frams/util/3d.cpp
===================================================================
--- cpp/frams/util/3d.cpp	(revision 197)
+++ cpp/frams/util/3d.cpp	(revision 255)
@@ -7,205 +7,227 @@
 #include "3d.h"
 
-Pt3D operator+(const Pt3D &p1,const Pt3D &p2) {return Pt3D(p1.x+p2.x,p1.y+p2.y,p1.z+p2.z);}
-Pt3D operator-(const Pt3D &p1,const Pt3D &p2) {return Pt3D(p1.x-p2.x,p1.y-p2.y,p1.z-p2.z);}
-
-Pt3D Pt3D_0(0,0,0);
-
-bool Pt3D::report_errors=true;
+Pt3D operator+(const Pt3D &p1, const Pt3D &p2) { return Pt3D(p1.x + p2.x, p1.y + p2.y, p1.z + p2.z); }
+Pt3D operator-(const Pt3D &p1, const Pt3D &p2) { return Pt3D(p1.x - p2.x, p1.y - p2.y, p1.z - p2.z); }
+
+Pt3D Pt3D_0(0, 0, 0);
+
+bool Pt3D::report_errors = true;
 
 double Pt3D::operator()() const
 {
-double q=x*x+y*y+z*z;
-if (q<0) {if (report_errors) FramMessage("Pt3D","operator()","sqrt domain error",3); return 0;}
-return sqrt(q);
+	double q = x*x + y*y + z*z;
+	if (q < 0) { if (report_errors) FramMessage("Pt3D", "operator()", "sqrt domain error", 3); return 0; }
+	return sqrt(q);
 }
 
 bool Pt3D::normalize()
 {
-double len=length();
-if (fabs(len)<1e-50) {if (report_errors) FramMessage("Pt3D","normalize()","vector too small",1); x=1;y=0;z=0; return false;}
-operator/=(len);
-return true;
+	double len = length();
+	if (fabs(len) < 1e-50) { if (report_errors) FramMessage("Pt3D", "normalize()", "vector too small", 1); x = 1; y = 0; z = 0; return false; }
+	operator/=(len);
+	return true;
 }
 
 double Pt3D::distanceTo(const Pt3D& p) const
 {
-return sqrt((x-p.x)*(x-p.x)+(y-p.y)*(y-p.y)+(z-p.z)*(z-p.z));
+	return sqrt((x - p.x)*(x - p.x) + (y - p.y)*(y - p.y) + (z - p.z)*(z - p.z));
 }
 
 double Pt3D::manhattanDistanceTo(const Pt3D& p) const
 {
-return fabs(x-p.x)+fabs(y-p.y)+fabs(z-p.z);
-}
-
-Orient Orient_1(Pt3D(1,0,0),Pt3D(0,1,0),Pt3D(0,0,1));
+	return fabs(x - p.x) + fabs(y - p.y) + fabs(z - p.z);
+}
+
+Orient Orient_1(Pt3D(1, 0, 0), Pt3D(0, 1, 0), Pt3D(0, 0, 1));
 
 // prosty obrot
-void rotate2D(double k,double &x,double &y)
-{double s=sin(k),c=cos(k);
-double t=c*x-s*y; y=s*x+c*y; x=t;}
-
-void rotate2D(double s,double c,double &x,double &y)
-{double t=c*x-s*y; y=s*x+c*y; x=t;}
-
-int Pt3D::getAngle(double dx,double dy,double &wyn)
-{
-if ((fabs(dx)+fabs(dy))<0.0001) return 0;
-wyn=atan2(dy,dx);
-return 1;
-}
-
-void Pt3D::getAngles(const Pt3D& X,const Pt3D& dir)
-{
-Pt3D	t1(X), t2(dir);
-if (getAngle(t1.x,t1.y,z))
-	{	// nie pionowo
-	rotate2D(-z,t1.x,t1.y);
-	rotate2D(-z,t2.x,t2.y);
-	getAngle(t1.x, t1.z, y);
-	}
-else
-	{	// pionowo
-	z=0;
-	if (t1.z<0)
-		y=-M_PI_2; // w dol
-	else
-		y=M_PI_2; // w gore
-	}
-rotate2D(-y,t2.x,t2.z);
-if (!getAngle(t2.z,-t2.y,x))
-	x=0;// to zly wynik ale dobrego nie ma :P
+void rotate2D(double k, double &x, double &y)
+{
+	double s = sin(k), c = cos(k);
+	double t = c*x - s*y; y = s*x + c*y; x = t;
+}
+
+void rotate2D(double s, double c, double &x, double &y)
+{
+	double t = c*x - s*y; y = s*x + c*y; x = t;
+}
+
+int Pt3D::getAngle(double dx, double dy, double &wyn)
+{
+	if ((fabs(dx) + fabs(dy)) < 0.0001) return 0;
+	wyn = atan2(dy, dx);
+	return 1;
+}
+
+void Pt3D::getAngles(const Pt3D& X, const Pt3D& dir)
+{
+	Pt3D	t1(X), t2(dir);
+	if (getAngle(t1.x, t1.y, z)) // non-vertical
+	{	
+		rotate2D(-z, t1.x, t1.y);
+		rotate2D(-z, t2.x, t2.y);
+		getAngle(t1.x, t1.z, y);
+	}
+	else // vertical
+	{	
+		z = 0;
+		if (t1.z < 0)
+			y = -M_PI_2; // down
+		else
+			y = M_PI_2; // up
+	}
+	rotate2D(-y, t2.x, t2.z);
+	if (!getAngle(t2.z, -t2.y, x))
+		x = 0; // incorrect result, but there is no correct one
 }
 
 void Pt3D::getMin(const Pt3D& p)
 {
-if (p.x<x) x=p.x;
-if (p.y<y) y=p.y;
-if (p.z<z) z=p.z;
+	if (p.x < x) x = p.x;
+	if (p.y < y) y = p.y;
+	if (p.z < z) z = p.z;
 }
 void Pt3D::getMax(const Pt3D& p)
 {
-if (p.x>x) x=p.x;
-if (p.y>y) y=p.y;
-if (p.z>z) z=p.z;
-}
-
-void Pt3D::vectorProduct(const Pt3D& a,const Pt3D& b)
-{
-x=a.y*b.z-a.z*b.y;
-y=a.z*b.x-a.x*b.z;
-z=a.x*b.y-a.y*b.x;
-}
-
-void Orient::lookAt(const Pt3D& X,const Pt3D& dir)
-{
-x=X; x.normalize();
-y.vectorProduct(dir,x);
-z.vectorProduct(x,y);
-if ((!y.normalize()) || (!z.normalize()))
-	{
-	y.x=dir.y; y.y=dir.z; y.z=dir.x;
-	z.vectorProduct(x,y);
-	z.normalize();
-	}
-}
-
-// odleglosc 2d
-double d2(double x,double y)
-{
-double q=x*x+y*y;
-if (q<0) {if (Pt3D::report_errors) FramMessage("","d2()","sqrt domain error",3); return 0;}
-return sqrt(q);
+	if (p.x > x) x = p.x;
+	if (p.y > y) y = p.y;
+	if (p.z > z) z = p.z;
+}
+
+void Pt3D::vectorProduct(const Pt3D& a, const Pt3D& b)
+{
+	x = a.y*b.z - a.z*b.y;
+	y = a.z*b.x - a.x*b.z;
+	z = a.x*b.y - a.y*b.x;
+}
+
+void Orient::lookAt(const Pt3D& X, const Pt3D& dir)
+{
+	x = X; x.normalize();
+	y.vectorProduct(dir, x);
+	z.vectorProduct(x, y);
+	if ((!y.normalize()) || (!z.normalize()))
+		lookAt(X);// dir was (nearly?) parallel, there is no good solution, use the x-only variant
+}
+
+void Orient::lookAt(const Pt3D& X)
+{
+	x = X; x.normalize();
+	// "invent" y vector, not parallel to x
+	double ax = fabs(x.x), ay = fabs(x.y), az = fabs(x.z);
+	// find the smallest component
+	if ((ax <= ay) && (ax <= az)) // x
+	{
+		y.x = 0; y.y = -x.z; y.z = x.y; // (0,-z,y)
+	} 
+	if ((ay <= ax) && (ay <= az)) // y
+	{
+		y.x = -x.z; y.y = 0; y.z = x.x; // (-z,0,x)
+	} 
+	else // z
+	{
+		y.x = -x.y; y.y = x.x; y.z = 0; // (-y,x,0)
+	} 
+	y.normalize();
+	z.vectorProduct(x, y);
+}
+
+// 2D distance
+double d2(double x, double y)
+{
+	double q = x*x + y*y;
+	if (q < 0) { if (Pt3D::report_errors) FramMessage("", "d2()", "sqrt domain error", 3); return 0; }
+	return sqrt(q);
 }
 
 Orient::Orient(const Matrix44& m)
 {
-x.x=m[0];  x.y=m[1];  x.z=m[2];
-y.x=m[4];  y.y=m[5];  y.z=m[6];
-z.x=m[8];  z.y=m[9];  z.z=m[10];
+	x.x = m[0];  x.y = m[1];  x.z = m[2];
+	y.x = m[4];  y.y = m[5];  y.z = m[6];
+	z.x = m[8];  z.y = m[9];  z.z = m[10];
 }
 
 void Orient::operator=(const Pt3D &rot)
 {
-*this=Orient_1;
-rotate(rot);
+	*this = Orient_1;
+	rotate(rot);
 }
 
 void Orient::rotate(const Pt3D &v)
 {
-double s,c;
-if (fabs(v.x)>0.0001)
-	{
-	s=sin(v.x); c=cos(v.x);
-	rotate2D(s,c,x.y,x.z);
-	rotate2D(s,c,y.y,y.z);
-	rotate2D(s,c,z.y,z.z);
-	}
-if (fabs(v.y)>0.0001)
-	{
-	s=sin(v.y); c=cos(v.y);
-	rotate2D(s,c,x.x,x.z);
-	rotate2D(s,c,y.x,y.z);
-	rotate2D(s,c,z.x,z.z);
-	}
-if (fabs(v.z)>0.0001)
-	{
-	s=sin(v.z); c=cos(v.z);
-	rotate2D(s,c,x.x,x.y);
-	rotate2D(s,c,y.x,y.y);
-	rotate2D(s,c,z.x,z.y);
-	}
-}
-
-void Orient::transform(Pt3D& target,const Pt3D &s) const
-{
-target.x=s.x*x.x+s.y*y.x+s.z*z.x;
-target.y=s.x*x.y+s.y*y.y+s.z*z.y;
-target.z=s.x*x.z+s.y*y.z+s.z*z.z;
-}
-
-void Orient::revTransform(Pt3D& target,const Pt3D &s) const
-{
-target.x=s.x*x.x+s.y*x.y+s.z*x.z;
-target.y=s.x*y.x+s.y*y.y+s.z*y.z;
-target.z=s.x*z.x+s.y*z.y+s.z*z.z;
-}
-
-void Orient::transform(Orient& target,const Orient& src) const
-{
-transform(target.x,src.x);
-transform(target.y,src.y);
-transform(target.z,src.z);
-}
-
-void Orient::revTransform(Orient& target,const Orient& src) const
-{
-revTransform(target.x,src.x);
-revTransform(target.y,src.y);
-revTransform(target.z,src.z);
+	double s, c;
+	if (fabs(v.x) > 0.0001)
+	{
+		s = sin(v.x); c = cos(v.x);
+		rotate2D(s, c, x.y, x.z);
+		rotate2D(s, c, y.y, y.z);
+		rotate2D(s, c, z.y, z.z);
+	}
+	if (fabs(v.y) > 0.0001)
+	{
+		s = sin(v.y); c = cos(v.y);
+		rotate2D(s, c, x.x, x.z);
+		rotate2D(s, c, y.x, y.z);
+		rotate2D(s, c, z.x, z.z);
+	}
+	if (fabs(v.z) > 0.0001)
+	{
+		s = sin(v.z); c = cos(v.z);
+		rotate2D(s, c, x.x, x.y);
+		rotate2D(s, c, y.x, y.y);
+		rotate2D(s, c, z.x, z.y);
+	}
+}
+
+void Orient::transform(Pt3D& target, const Pt3D &s) const
+{
+	target.x = s.x*x.x + s.y*y.x + s.z*z.x;
+	target.y = s.x*x.y + s.y*y.y + s.z*z.y;
+	target.z = s.x*x.z + s.y*y.z + s.z*z.z;
+}
+
+void Orient::revTransform(Pt3D& target, const Pt3D &s) const
+{
+	target.x = s.x*x.x + s.y*x.y + s.z*x.z;
+	target.y = s.x*y.x + s.y*y.y + s.z*y.z;
+	target.z = s.x*z.x + s.y*z.y + s.z*z.z;
+}
+
+void Orient::transform(Orient& target, const Orient& src) const
+{
+	transform(target.x, src.x);
+	transform(target.y, src.y);
+	transform(target.z, src.z);
+}
+
+void Orient::revTransform(Orient& target, const Orient& src) const
+{
+	revTransform(target.x, src.x);
+	revTransform(target.y, src.y);
+	revTransform(target.z, src.z);
 }
 
 void Orient::getAngles(Pt3D &angles) const
 {
-angles.getAngles(x,z);
+	angles.getAngles(x, z);
 }
 
 bool Orient::normalize()
 {
-bool ret=1;
-y.vectorProduct(z,x);
-z.vectorProduct(x,y);
-if (!x.normalize()) ret=0;
-if (!z.normalize()) ret=0;
-if (!y.normalize()) ret=0;
-return ret;
+	bool ret = 1;
+	y.vectorProduct(z, x);
+	z.vectorProduct(x, y);
+	if (!x.normalize()) ret = 0;
+	if (!z.normalize()) ret = 0;
+	if (!y.normalize()) ret = 0;
+	return ret;
 }
 
 Matrix44::Matrix44(const Orient &rot)
 {
-m[0]=rot.x.x;  m[1]=rot.x.y;  m[2]=rot.x.z;  m[3]=0;
-m[4]=rot.y.x;  m[5]=rot.y.y;  m[6]=rot.y.z;  m[7]=0;
-m[8]=rot.z.x;  m[9]=rot.z.y;  m[10]=rot.z.z; m[11]=0;
-m[12]=0;       m[13]=0;       m[14]=0;       m[15]=1;
+	m[0] = rot.x.x;  m[1] = rot.x.y;  m[2] = rot.x.z;  m[3] = 0;
+	m[4] = rot.y.x;  m[5] = rot.y.y;  m[6] = rot.y.z;  m[7] = 0;
+	m[8] = rot.z.x;  m[9] = rot.z.y;  m[10] = rot.z.z; m[11] = 0;
+	m[12] = 0;       m[13] = 0;       m[14] = 0;       m[15] = 1;
 }
 
