Index: mbdyn/struct/planej.cc
===================================================================
RCS file: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/planej.cc,v
retrieving revision 1.90
diff -u -r1.90 planej.cc
--- mbdyn/struct/planej.cc	22 May 2010 18:16:04 -0000	1.90
+++ mbdyn/struct/planej.cc	18 Aug 2010 18:53:21 -0000
@@ -39,6 +39,7 @@
 #include <limits>
 
 #include "planej.h"
+#include "Rot.hh"
 #include "hint_impl.h"
 
 /* PlaneHingeJoint - begin */
@@ -62,7 +63,7 @@
 Joint(uL, pDO, fOut), 
 pNode1(pN1), pNode2(pN2),
 d1(dTmp1), R1h(R1hTmp), d2(dTmp2), R2h(R2hTmp), F(0.), M(0.),
-calcInitdTheta(_calcInitdTheta), dTheta(initDTheta),
+calcInitdTheta(_calcInitdTheta), NTheta(0), dTheta(initDTheta), dThetaWrapped(initDTheta),
 Sh_c(sh), fc(f), preF(pref), r(rr)
 {
 	NO_OP;
@@ -401,10 +402,9 @@
 	}
 
 	if (calcInitdTheta) {
-		Mat3x3 RTmp((pNode1->GetRCurr()*R1h).Transpose()*(pNode2->GetRCurr()*R2h));
-		Vec3 v(MatR2EulerAngles(RTmp));
+		Vec3 v(RotManip::VecRot((pNode1->GetRCurr()*R1h).MulTM(pNode2->GetRCurr()*R2h)));
 		
-		dTheta = v.dGet(3);
+		dThetaWrapped = dTheta = v.dGet(3);
 	}
 	
 #if 0
@@ -467,14 +467,23 @@
 PlaneHingeJoint::AfterConvergence(const VectorHandler& X, 
 		const VectorHandler& XP)
 {
+	Vec3 v(RotManip::VecRot((pNode1->GetRCurr()*R1h).MulTM(pNode2->GetRCurr()*R2h)));
+	doublereal dThetaTmp(v(3));
 
-	Mat3x3 RTmp(((pNode1->GetRCurr()*R1h).Transpose()
-			*pNode1->GetRPrev()*R1h).Transpose()
-			*((pNode2->GetRCurr()*R2h).Transpose()
-			*pNode2->GetRPrev()*R2h));
-	Vec3 v(MatR2EulerAngles(RTmp.Transpose()));
+	// unwrap
+	if (dThetaTmp - dThetaWrapped < -M_PI) {
+		NTheta++;
+	}
+
+	if (dThetaTmp - dThetaWrapped > M_PI) {
+		NTheta--;
+	}
 
-	dTheta += v.dGet(3);
+	// save new wrapped angle
+	dThetaWrapped = dThetaTmp;
+
+	// compute new unwrapped angle
+	dTheta = 2*M_PI*NTheta + dThetaWrapped;
 
 	if (fc) {
 		Mat3x3 R1(pNode1->GetRCurr());
@@ -1361,13 +1370,20 @@
    
    switch (i) {
     case 1: {
-       Mat3x3 RTmp(((pNode1->GetRCurr()*R1h).Transpose()
-			*pNode1->GetRPrev()*R1h).Transpose()
-			*((pNode2->GetRCurr()*R2h).Transpose()
-			*pNode2->GetRPrev()*R2h));
-       Vec3 v(MatR2EulerAngles(RTmp.Transpose()));
+	Vec3 v(RotManip::VecRot((pNode1->GetRCurr()*R1h).MulTM(pNode2->GetRCurr()*R2h)));
+	doublereal dThetaTmp(v(3));
+
+	int n = 0;
+
+	if (dThetaTmp - dThetaWrapped < -M_PI) {
+		n++;
+	}
+
+	if (dThetaTmp - dThetaWrapped > M_PI) {
+		n--;
+	}
 
-       return dTheta + v(3);
+	return 2*M_PI*(NTheta + n) + dThetaTmp;
     }
       
     case 2: {
@@ -1406,7 +1422,8 @@
 : Elem(uL, fOut), 
 Joint(uL, pDO, fOut), 
 pNode1(pN1), pNode2(pN2),
-R1h(R1hTmp), R2h(R2hTmp), M(0.), dTheta(0.)
+R1h(R1hTmp), R2h(R2hTmp), M(0.),
+NTheta(0), dTheta(0.), dThetaWrapped(0.)
 {
    NO_OP;
 }
@@ -1600,10 +1617,9 @@
 		}
 	}
 
-	Mat3x3 RTmp((pNode1->GetRCurr()*R1h).Transpose()*(pNode2->GetRCurr()*R2h));
-	Vec3 v(MatR2EulerAngles(RTmp));
+	Vec3 v(RotManip::VecRot((pNode1->GetRCurr()*R1h).MulTM(pNode2->GetRCurr()*R2h)));
 
-	dTheta = v.dGet(3);
+	dThetaWrapped = dTheta = v.dGet(3);
 }
 
 Hint *
@@ -1632,13 +1648,23 @@
 PlaneRotationJoint::AfterConvergence(const VectorHandler& X, 
 		const VectorHandler& XP)
 {
-	Mat3x3 RTmp(((pNode1->GetRCurr()*R1h).Transpose()
-			*pNode1->GetRPrev()*R1h).Transpose()
-			*((pNode2->GetRCurr()*R2h).Transpose()
-			*pNode2->GetRPrev()*R2h));
-	Vec3 v(MatR2EulerAngles(RTmp.Transpose()));
+	Vec3 v(RotManip::VecRot((pNode1->GetRCurr()*R1h).MulTM(pNode2->GetRCurr()*R2h)));
+	doublereal dThetaTmp(v(3));
+
+	// unwrap
+	if (dThetaTmp - dThetaWrapped < -M_PI) {
+		NTheta++;
+	}
+
+	if (dThetaTmp - dThetaWrapped > M_PI) {
+		NTheta--;
+	}
 
-	dTheta += v.dGet(3);
+	// save new wrapped angle
+	dThetaWrapped = dThetaTmp;
+
+	// compute new unwrapped angle
+	dTheta = 2*M_PI*NTheta + dThetaWrapped;
 }
 
 
@@ -2237,13 +2263,20 @@
    
    switch (i) {
     case 1: {
-       Mat3x3 RTmp(((pNode1->GetRCurr()*R1h).Transpose()
-			*pNode1->GetRPrev()*R1h).Transpose()
-			*((pNode2->GetRCurr()*R2h).Transpose()
-			*pNode2->GetRPrev()*R2h));
-       Vec3 v(MatR2EulerAngles(RTmp.Transpose()));
+	Vec3 v(RotManip::VecRot((pNode1->GetRCurr()*R1h).MulTM(pNode2->GetRCurr()*R2h)));
+	doublereal dThetaTmp(v(3));
+
+	int n = 0;
+
+	if (dThetaTmp - dThetaWrapped < -M_PI) {
+		n++;
+	}
+
+	if (dThetaTmp - dThetaWrapped > M_PI) {
+		n--;
+	}
 
-       return dTheta + v(3);
+	return 2*M_PI*(NTheta + n) + dThetaTmp;
     }
       
     case 2: {
@@ -2288,7 +2321,8 @@
 Joint(uL, pDO, fOut), 
 DriveOwner(pDC), 
 pNode1(pN1), pNode2(pN2), 
-d1(dTmp1), R1h(R1hTmp), d2(dTmp2), R2h(R2hTmp), F(0.), M(0.), dTheta(0.),
+d1(dTmp1), R1h(R1hTmp), d2(dTmp2), R2h(R2hTmp), F(0.), M(0.),
+NTheta(0), dTheta(0.), dThetaWrapped(0.),
 Sh_c(sh), fc(f), preF(pref), r(rr)
 {
 	NO_OP;
@@ -2646,7 +2680,7 @@
 	Mat3x3 RTmp((pNode1->GetRCurr()*R1h).Transpose()*(pNode2->GetRCurr()*R2h));
 	Vec3 v(MatR2EulerAngles(RTmp));
 
-	dTheta = v.dGet(3);
+	dThetaWrapped = dTheta = v.dGet(3);
 	
 	if (fc) {
 		fc->SetValue(pDM, X, XP, ph, iGetFirstIndex() + NumSelfDof);
@@ -2697,12 +2731,24 @@
 AxialRotationJoint::AfterConvergence(const VectorHandler& X, 
 		const VectorHandler& XP)
 {
-	Mat3x3 R1Tmp(((pNode1->GetRCurr()*R1h).Transpose()*pNode1->GetRPrev()*R1h).Transpose()
-		*((pNode2->GetRCurr()*R2h).Transpose()*pNode2->GetRPrev()*R2h));
-	Vec3 v1(MatR2EulerAngles(R1Tmp.Transpose()));
+	Vec3 v(RotManip::VecRot((pNode1->GetRCurr()*R1h).MulTM(pNode2->GetRCurr()*R2h)));
+	doublereal dThetaTmp(v(3));
+
+	// unwrap
+	if (dThetaTmp - dThetaWrapped < -M_PI) {
+		NTheta++;
+	}
+
+	if (dThetaTmp - dThetaWrapped > M_PI) {
+		NTheta--;
+	}
+
+	// save new wrapped angle
+	dThetaWrapped = dThetaTmp;
+
+	// compute new unwrapped angle
+	dTheta = 2*M_PI*NTheta + dThetaWrapped;
 
-	dTheta += v1.dGet(3);
-	
 	if (fc) {
 		Mat3x3 R1(pNode1->GetRCurr());
 		Mat3x3 R1hTmp(R1*R1h);
@@ -3559,13 +3605,20 @@
    
 	switch (i) {
 	case 1: {
-		Mat3x3 RTmp(((pNode1->GetRCurr()*R1h).Transpose()
-				*pNode1->GetRPrev()*R1h).Transpose()
-				*((pNode2->GetRCurr()*R2h).Transpose()
-					*pNode2->GetRPrev()*R2h));
-		Vec3 v(MatR2EulerAngles(RTmp.Transpose()));
+		Vec3 v(RotManip::VecRot((pNode1->GetRCurr()*R1h).MulTM(pNode2->GetRCurr()*R2h)));
+		doublereal dThetaTmp(v(3));
+
+		int n = 0;
 
-		return dTheta + v(3);
+		if (dThetaTmp - dThetaWrapped < -M_PI) {
+			n++;
+		}
+
+		if (dThetaTmp - dThetaWrapped > M_PI) {
+			n--;
+		}
+
+		return 2*M_PI*(NTheta + n) + dThetaTmp;
 	}
       
 	case 2: 
@@ -3604,7 +3657,8 @@
 pNode(pN), 
 X0(X0Tmp), R0(R0Tmp), d(dTmp), Rh(RhTmp),
 F(0.), M(0.),
-calcInitdTheta(_calcInitdTheta), dTheta(initDTheta)
+calcInitdTheta(_calcInitdTheta),
+NTheta(0), dTheta(initDTheta), dThetaWrapped(0.)
 {
    NO_OP;
 }
@@ -3870,9 +3924,8 @@
 	}
 
 	if (calcInitdTheta) {
-		Mat3x3 RTmp(R0.Transpose()*(pNode->GetRCurr()*Rh));
-		Vec3 v(MatR2EulerAngles(RTmp));
-		dTheta = v.dGet(3);
+		Vec3 v(RotManip::VecRot(R0.MulTM(pNode->GetRCurr()*Rh)));
+		dThetaWrapped = dTheta = v.dGet(3);
 	}
 }
 
@@ -3925,10 +3978,24 @@
 PlanePinJoint::AfterConvergence(const VectorHandler& X, 
 		const VectorHandler& XP)
 {
-	Mat3x3 RTmp(pNode->GetRPrev().Transpose()*pNode->GetRCurr()*Rh);
-	Vec3 v(MatR2EulerAngles(RTmp));
+	Vec3 v(RotManip::VecRot(R0.MulTM(pNode->GetRCurr()*Rh)));
+	doublereal dThetaTmp(v(3));
+
+	// unwrap
+	if (dThetaTmp - dThetaWrapped < -M_PI) {
+		NTheta++;
+	}
+
+	if (dThetaTmp - dThetaWrapped > M_PI) {
+		NTheta--;
+	}
+
+	// save new wrapped angle
+	dThetaWrapped = dThetaTmp;
+
+	// compute new unwrapped angle
+	dTheta = 2*M_PI*NTheta + dThetaWrapped;
 
-	dTheta += v.dGet(3);
 }
 
 void
@@ -4440,10 +4507,20 @@
    
    switch (i) {
     case 1: {
-	Mat3x3 RTmp(pNode->GetRPrev().Transpose()*pNode->GetRCurr()*Rh);
-	Vec3 v(MatR2EulerAngles(RTmp));
+	Vec3 v(RotManip::VecRot(R0.MulTM(pNode->GetRCurr()*Rh)));
+	doublereal dThetaTmp(v(3));
+
+	int n = 0;
+
+	if (dThetaTmp - dThetaWrapped < -M_PI) {
+		n++;
+	}
+
+	if (dThetaTmp - dThetaWrapped > M_PI) {
+		n--;
+	}
 
-       return dTheta + v(3);
+	return 2*M_PI*(NTheta + n) + dThetaTmp;
     }
       
     case 2: {
Index: mbdyn/struct/planej.h
===================================================================
RCS file: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/planej.h,v
retrieving revision 1.42
diff -u -r1.42 planej.h
--- mbdyn/struct/planej.h	13 Feb 2010 00:00:35 -0000	1.42
+++ mbdyn/struct/planej.h	18 Aug 2010 18:53:21 -0000
@@ -59,7 +59,8 @@
    Vec3 F;
    Vec3 M;
    bool calcInitdTheta;
-   mutable doublereal dTheta;
+   mutable int NTheta;
+   mutable doublereal dTheta, dThetaWrapped;
 
    /* friction related data */
    BasicShapeCoefficient *const Sh_c;
@@ -204,7 +205,8 @@
    Mat3x3 R1h;
    Mat3x3 R2h;
    Vec3 M;
-   mutable doublereal dTheta;
+   mutable int NTheta;
+   mutable doublereal dTheta, dThetaWrapped;
    
  public:
    /* Costruttore non banale */
@@ -337,7 +339,8 @@
    Mat3x3 R2h;
    Vec3 F;
    Vec3 M;
-   mutable doublereal dTheta;
+   mutable int NTheta;
+   mutable doublereal dTheta, dThetaWrapped;
 
    /* friction related data */
    BasicShapeCoefficient *const Sh_c;
@@ -489,7 +492,8 @@
    Vec3 F;
    Vec3 M;
    bool calcInitdTheta;
-   mutable doublereal dTheta;
+   mutable int NTheta;
+   mutable doublereal dTheta, dThetaWrapped;
    
  public:
    /* Costruttore non banale */