diff -Nu librpp/arGetInitRot2.cpp librpp.new/arGetInitRot2.cpp
--- librpp/arGetInitRot2.cpp	2009-11-08 00:10:58.578313359 +0100
+++ librpp.new/arGetInitRot2.cpp	2008-01-30 21:36:40.000000000 +0100
@@ -39,7 +39,7 @@
 
 
 
-#include "arGetInitRot2Sub.h"
+#include "../librpp/arGetInitRot2Sub.h"
 
 
 namespace ARToolKitPlus {
diff -Nu librpp/librpp.cpp librpp.new/librpp.cpp
--- librpp/librpp.cpp	2009-09-26 23:39:40.562817878 +0200
+++ librpp.new/librpp.cpp	2008-01-30 21:36:40.000000000 +0100
@@ -36,11 +36,13 @@
  * ========================================================================
  ** @author   Thomas Pintaric
  *
- * $Id: librpp.cpp 162 2006-04-19 21:28:10Z grabner $
+ * $Id: librpp.cpp 198 2006-11-01 16:00:04Z daniel $
  * @file
  * ======================================================================== */
 
 
+#include <ARToolKitPlus/OS.h>
+
 #include "librpp.h"
 #include "rpp.h"
 #include "rpp_vecmat.h"
@@ -145,6 +147,7 @@
 
 #else //_NO_LIBRPP_
 
+#pragma message("Compiling without RPP support")
 
 LIBRPP_API void robustPlanarPose(rpp_float &err,
 								 rpp_mat &R,
@@ -160,6 +163,21 @@
 								 const rpp_float tolerance,
 								 const unsigned int max_iterations)
 {
+	// prevent warnings about unused parameters...
+	//
+	err;
+	R;
+	t;
+	cc;
+	fc;
+	model;
+	iprts;
+	model_iprts_size;
+	R_init;
+	estimate_R_init;
+	epsilon;
+	tolerance;
+	max_iterations;
 }
 
 
diff -Nu librpp/librpp.h librpp.new/librpp.h
--- librpp/librpp.h	2009-09-26 14:43:46.558816735 +0200
+++ librpp.new/librpp.h	2008-01-30 21:36:40.000000000 +0100
@@ -36,12 +36,13 @@
  * ========================================================================
  ** @author   Thomas Pintaric
  *
- * $Id: librpp.h 162 2006-04-19 21:28:10Z grabner $
+ * $Id: librpp.h 180 2006-08-20 16:57:22Z daniel $
  * @file
  * ======================================================================== */
 
 #if defined(_MSC_VER) || defined(_WIN32_WCE)
 
+
 #ifdef LIBRPP_STATIC
 #  define LIBRPP_API
 #elif LIBRPP_DLL
@@ -60,9 +61,11 @@
 
 #endif
 
-typedef double rpp_float;
-typedef double rpp_vec[3];
-typedef double rpp_mat[3][3];
+//typedef double rpp_float;
+//typedef double rpp_vec[3];
+//typedef double rpp_mat[3][3];
+
+#include "rpp_types.h"
 
 LIBRPP_API void robustPlanarPose(rpp_float &err,
 								 rpp_mat &R,
Gemeinsame Unterverzeichnisse: librpp/MATLAB und librpp.new/MATLAB.
diff -Nu librpp/rpp.cpp librpp.new/rpp.cpp
--- librpp/rpp.cpp	2009-09-26 23:39:40.562817878 +0200
+++ librpp.new/rpp.cpp	2008-01-30 21:36:40.000000000 +0100
@@ -36,21 +36,67 @@
  * ========================================================================
  ** @author   Thomas Pintaric
  *
- * $Id: rpp.cpp 162 2006-04-19 21:28:10Z grabner $
+ * $Id: rpp.cpp 199 2006-11-02 13:52:52Z schall $
  * @file
  * ======================================================================== */
 
 
+#include <ARToolKitPlus/OS.h>
+
+
 #ifndef _NO_LIBRPP_
 
+
+#include <ARToolKitPlus/extra/stl_vector.h>
+#include <assert.h>
+//#include <memory.h>
+
+
+//#include <iostream.h>
+
+//  #define deb(a) std::cout << "libRpp " << a << std::endl;
+
+#ifdef deb
+#define print_R(s,R) printf("libRpp %s=[",s);				 \
+  printf("[%.15f %.15f %.15f]\n",R.m[0][0],R.m[0][1],R.m[0][2]); \
+  printf("[%.15f %.15f %.15f]\n",R.m[1][0],R.m[1][1],R.m[1][2]); \
+  printf("[%.15f %.15f %.15f]];\n",R.m[2][0],R.m[2][1],R.m[2][2])
+
+#define print_t(s,t) printf("libRpp %s=[",s);				\
+  printf("[%.15f %.15f %.15f]]';\n",t.v[0],t.v[1],t.v[2])
+
+#define print_model(n,P)  printf("libRpp ");for(unsigned int __i=0;__i<P.size();__i++) \
+    printf("%s(:,%d) = [%.15f %.15f %.15f ]';\n",n,__i+1,P[__i].v[0],P[__i].v[1],P[__i].v[2])
+
+#define disp(name,Rlu) std::cout <<"libRpp " <<  name << Rlu.m[0][0] << "," << Rlu.m[0][1] << "," << Rlu.m[0][2] << std::endl; \
+    std::cout << name << Rlu.m[1][0] << "," << Rlu.m[1][1] << "," << Rlu.m[1][2] << std::endl; \
+    std::cout << name << Rlu.m[2][0] << "," << Rlu.m[2][1] << "," << Rlu.m[2][2] << std::endl
+
+#else
+ #define print_R(s,R)
+ #define print_t(s,t)
+ #define print_model(n,P)
+ #define disp(name,Rlu)
+ #define deb(a) 
+#endif
+
+//#include "Utility/PerformanceAnalyser.h"
+//#include "Utility/XMLNode.h"
+
 #include <vector>
-#include <cstring>
-#include <cassert>
+#include "assert.h"
+
+
+
+
+
 
 #include "rpp.h"
 #include "rpp_const.h"
 #include "rpp_vecmat.h"
 
+#include <math.h>
+
 namespace rpp {
 
 // ===========================================================================================
@@ -69,12 +115,28 @@
 
 void GetRotationbyVector(mat33_t &R, const vec3_t &v1, const vec3_t &v2)
 {
-	real_t winkel = _acos(vec3_dot(v1,v2));
-	quat_t QU;
-	vec3_t vc;
-	vec3_cross(vc,v2,v1);
-	Quaternion_byAngleAndVector(QU,winkel,vc);
-	mat33_from_quat(R,QU);
+	 vec3_t diff;
+  vec3_sub(diff,v1,v2);
+    
+  // std::cout << "v1:" << v1.v[0] << "," <<  v1.v[1] << "," << v1.v[2] << std::endl;
+  // std::cout << "v2:" << v2.v[0] << "," <<  v2.v[1] << "," << v2.v[2] << std::endl;
+   
+  // std::cout << "Diff:" << vec3_norm(diff) << std::endl;
+  if( vec3_norm(diff) < 1e-3 ){
+    mat33_assign(R,1,0,0,0,1,0,0,0,1);
+  }else
+    if( fabs((vec3_norm(diff)-2)) < 1e-3 ){
+      mat33_assign(R,1,0,0 ,0,-1,0, 0,0,-1);
+    }else
+      { 
+	real_t winkel = _acos(vec3_dot(v1,v2));
+	quat_t QU;
+	vec3_t vc;
+	vec3_cross(vc,v2,v1);
+	Quaternion_byAngleAndVector(QU,winkel,vc);
+	mat33_from_quat(R,QU);
+	// disp("R:",R);
+      }
 }
 
 // ===========================================================================================
@@ -187,6 +249,32 @@
 	rpyMat(Rz,rpy);
 }
 
+// estimate the optimal t 
+// function t = estimate_t( R,G,F,P,n )
+//  sum_(1:3,1) = 0;
+//  for i = 1:n
+//    sum_ = sum_ + F(:,:,i)*R*P(:,i);
+//  end
+//  t = G*sum_;
+
+void optimal_t(vec3_t &t ,const mat33_t &R, const mat33_t &G,
+	       const mat33_array &F,const vec3_array &P,unsigned int n){
+  
+  vec3_t _sum;
+  vec3_clear(_sum);
+  
+  for(unsigned int i=0; i<n; i++)
+  {
+    vec3_t _v1,_v2;
+    vec3_mult(_v1,R,P.at(i));
+    vec3_mult(_v2,F.at(i),_v1);
+    vec3_add(_sum,_v2);
+  }
+  
+  vec3_mult(t,G,_sum);
+}
+
+
 // ===========================================================================================
 void abskernel(mat33_t &R, vec3_t &t, vec3_array &Qout, real_t &err2, 
 			   const vec3_array _P, const vec3_array _Q, 
@@ -237,17 +325,68 @@
 	mat33_transpose(_Ut,_U);
 	mat33_mult(R,_V,_Ut);
 
-	vec3_t _sum;
-	vec3_clear(_sum);
-	for(i=0; i<n; i++)
-	{
-		vec3_t _v1,_v2;
-		vec3_mult(_v1,R,P.at(i));
-		vec3_mult(_v2,F.at(i),_v1);
-		vec3_add(_sum,_v2);
+	// R = V*(U.');
+
+	// WE need to check the determinant of the R
+	real_t det = mat33_det(R);
+	
+	// std::cout << "det(R):=" << det << std::endl;
+	if ( det < 0 ){
+	  // determinant is negative -> no valid Rotation :-( 
+	  printf("Maybe this happens never : if it will-> check it if ok \n");
+	  
+	  // In the matlab code this happens often 
+	  // Maybe that that depends mostly on the used svd implementation :-( 
+	 
+	  // R=[V(:,1:2) -V(:,3)]*U.';
+	  mat33_t Vc; 
+	  Vc.m[0][0] = _V.m[0][0]; Vc.m[0][1] = _V.m[0][1]; Vc.m[0][2] = -_V.m[0][2];
+	  Vc.m[1][0] = _V.m[1][0]; Vc.m[1][1] = _V.m[1][1]; Vc.m[1][2] = -_V.m[1][2];
+	  Vc.m[2][0] = _V.m[2][0]; Vc.m[2][1] = _V.m[2][1]; Vc.m[2][2] = -_V.m[2][2];
+	  mat33_mult(R,Vc,_Ut);  // to have det(R) == 1
+	  optimal_t(t,R,G,F,P,n);
+	  if( t.v[2] < 0 ){
+	    Vc.m[0][0] =- _V.m[0][0]; Vc.m[0][1] =- _V.m[0][1]; Vc.m[0][2] = -_V.m[0][2];
+	    Vc.m[1][0] =- _V.m[1][0]; Vc.m[1][1] =- _V.m[1][1]; Vc.m[1][2] = -_V.m[1][2];
+	    Vc.m[2][0] =- _V.m[2][0]; Vc.m[2][1] =- _V.m[2][1]; Vc.m[2][2] = -_V.m[2][2];
+	    mat33_mult(R,Vc,_Ut);  // to have det(R) == 1 & t_3 > 0
+	    optimal_t(t,R,G,F,P,n);
+	  }
+	}else{
+	  // det(R) == 1
+	  // check the t 
+	  optimal_t(t,R,G,F,P,n);
+	  if( t.v[2] < 0 ){
+	    //   std::cout << "t_3 = " << t.v[2]  << std::endl;
+	    // generate a different R 
+	    //  R=-[V(:,1:2) -V(:,3)]*U.';
+	    mat33_t Vc; 
+	    Vc.m[0][0] = -_V.m[0][0]; Vc.m[0][1] = -_V.m[0][1]; Vc.m[0][2] = _V.m[0][2];
+            Vc.m[1][0] = -_V.m[1][0]; Vc.m[1][1] = -_V.m[1][1]; Vc.m[1][2] = _V.m[1][2];
+            Vc.m[2][0] = -_V.m[2][0]; Vc.m[2][1] = -_V.m[2][1]; Vc.m[2][2] = _V.m[2][2];
+	    mat33_mult(R,Vc,_Ut);
+	    optimal_t(t,R,G,F,P,n);
+	    
+	    //  print_R("V ",_V);
+	    // 	    print_R("U ",_U);
+	    // 	    print_R("R:",R);
+	    // 	    print_R("_S:",_S);
+	    
+	  }
+	}
+	
+	// CHECK if everything is working ok 
+	// check the det of R 
+	det = mat33_det(R);
+	if( det  < 0 ){
+	  printf("============== ERROR det= %d ============\n", det);
+	}
+	
+	// check the t 
+	if( t.v[2] < 0 ){
+	  printf("============== ERROR t(3) = << %d  <<  ============\n", t.v[2]);
+	  //usleep(1000000);
 	}
-
-	vec3_mult(t,G,_sum);
 	xform(Qout,P,R,t);
 	err2 = 0;
 	for(i=0; i<n; i++)
@@ -526,40 +665,78 @@
 	scalar_array e;
 	e.resize(num_sol);
 	scalar_array_clear(e);
-	scalar_array at;
-	scalar_array_add(e,_a[0]);
-	at.clear();
-	at.assign(at_sol.begin(),at_sol.end());
-	scalar_array_mult(at,_a[1]);
-	scalar_array_add(e,at);
-
-	for(j=2; j<=4; j++)
-	{
-		at.clear();
-		at.assign(at_sol.begin(),at_sol.end());
-		scalar_array_pow(at,real_t(j));
-		scalar_array_mult(at,_a[j]);
-		scalar_array_add(e,at);
-	}
-
-	scalar_array at_;
-	at_.clear();
-	for(i=0; i<at.size(); i++)
-	{
-		if(_abs(e[i]) < real_t(1e-3)) at_.push_back(at_sol[i]);
-	}
-
-	scalar_array p1(at_.begin(),at_.end());
-	scalar_array_pow(p1,2);
-	scalar_array_add(p1,1);
-	scalar_array_pow(p1,3);
-
-	at.clear();
-	for(i=0; i<at_.size(); i++)
-	{
-		if(_abs(p1[i]) > real_t(0.1f)) at.push_back(at_[i]);
-	}
-
+	
+	
+	
+	scalar_array at; at.resize(num_sol);
+	
+	if(1){
+	  // get the error in a complicate way
+	  scalar_array_clear(e);
+	  scalar_array_add(e,_a[0]);
+	  
+	  at.clear();
+	  at.assign(at_sol.begin(),at_sol.end());
+	  scalar_array_mult(at,_a[1]);
+	  scalar_array_add(e,at);
+	  
+	  for(j=2; j<=4; j++)
+	    {
+	      at.clear();
+	      at.assign(at_sol.begin(),at_sol.end());
+	      scalar_array_pow(at,real_t(j));
+	      scalar_array_mult(at,_a[j]);
+	      scalar_array_add(e,at);
+	    }
+	}else{
+	  // Or in a fast one 
+	  scalar_array_add(e,_a[4]);
+	  for(j=3;j>0;j--)
+	    // multiply with at & add a_ 
+	    for(unsigned int k=0;k<at_sol.size();k++)
+	      e[k] =  e[k]*at_sol[k] + _a[j] ;
+	}
+	
+	scalar_array at_; at_.clear();
+
+	bool all_ok = true;
+	for(i=0; i<e.size(); i++)
+	  {
+	    // Error of the polynomial ??
+	    deb("\n E[" << i << "]= " << _abs(e[i]) );
+	    
+	    // check if it is zero or equivalent  !!
+	    if( _abs(e[i] / at_sol[i]) < real_t(0.001)  || _abs(e[i]) < 0.001 ){
+	      at_.push_back(at_sol[i]);
+	      deb(" ok");
+	    }else{
+	      //  all_ok = false;
+	      at_.push_back(at_sol[i]);
+	    }
+	  }
+	
+	if( !all_ok){
+	  printf("ERROR: POLYGON EVALUATION FAILED !!! \n");
+	  //std::flush;
+	  exit(-1);
+	}
+	
+	// This is always true
+	// // check (1+at.^2)^3 > 0
+// 	scalar_array p1(at_.begin(),at_.end());
+// 	scalar_array_pow(p1,2);
+// 	scalar_array_add(p1,1);
+// 	// scalar_array_pow(p1,3); -> it's the same 
+// 	at.clear();
+// 	for(i=0; i<at_.size(); i++)
+// 	  {
+// 	    std::cout << "p1[" << i << "]= " << _abs(p1[i]) << std::endl;	
+// 	    if(_abs(p1[i]) > real_t(0.001f)) at.push_back(at_[i]);
+// 	  }
+	at.clear();
+	at.assign(at_.begin(),at_.end());
+
+	// get the angle al
 	scalar_array sa(at.begin(),at.end());
 	scalar_array_mult(sa,2);
 	scalar_array _ca1(at.begin(),at.end());
@@ -573,6 +750,8 @@
 	scalar_array_div(sa,_ca1);
 	scalar_array al;
 	scalar_array_atan2(al,sa,ca);
+	
+	// check the sign of the derivative 
 	scalar_array_mult(al,real_t(180./CONST_PI));
 	scalar_array _c_tMaxMin;
 	_c_tMaxMin.resize(at.size());
@@ -603,6 +782,7 @@
 		if(tMaxMin.at(i) > 0) al_.push_back(al.at(i));
 	}
 
+	// estimate R and t_opt for given angle 
 	tnew.resize(al_.size());
 	for(unsigned int a=0; a<al_.size(); a++)
 	{
@@ -658,6 +838,8 @@
 	scalar_array bl;
 	vec3_array Tnew;
 	getRotationY_wrtT(bl,Tnew, v ,P_, t, DB, Rz);
+	
+	// Estimate the Error for all solutions !
 	scalar_array_div(bl,180.0f/CONST_PI);
 	const unsigned int n = (unsigned int) v.size();
 	mat33_array V;
@@ -722,6 +904,8 @@
 	mat33_mult(R_,Rim,R);
 	vec3_mult(t_,Rim,t);
 	getRfor2ndPose_V_Exact(sol,v_,P,R_,t_,DB);
+	
+	// Re normalize it 
 	mat33_t Rim_tr;
 	mat33_transpose(Rim_tr,Rim);
 	for(unsigned int i=0; i<sol.size(); i++)
@@ -736,11 +920,16 @@
 	}
 }
 
-// =====================================================================================
-void robust_pose(real_t &err, mat33_t &R, vec3_t &t,
-		         const vec3_array &_model, const vec3_array &_iprts,
-		         const options_t _options)
-{
+// =====================================================================================
+// function returns two solutions to the pose problem (1st has the lower Error !!!!!!
+// ====================================================================================
+void robust_pose(real_t &err ,mat33_t & R ,vec3_t & t, 
+		 real_t &errb,mat33_t & Rb,vec3_t & tb,
+		 const vec3_array & _model,const vec3_array & _iprts,const options_t _options){
+ 
+  
+        print_R("robust_pose_initR:",_options.initR);
+  
 	mat33_t Rlu_;
 	vec3_t tlu_;
 	unsigned int it1_;
@@ -763,22 +952,28 @@
 	pose_vec sol;
 	sol.clear();
 	get2ndPose_Exact(sol,iprts,model,Rlu_,tlu_,0);
-	int min_err_idx = (-1);
-	real_t min_err = MAX_FLOAT;
-	for(unsigned int i=0; i<sol.size(); i++)
-	{
-		mat33_copy(options.initR,sol[i].R);
-		objpose(Rlu_, tlu_, it1_, obj_err1_, img_err1_, true, model, iprts, options);
-		mat33_copy(sol[i].PoseLu_R,Rlu_);
-		vec3_copy(sol[i].PoseLu_t,tlu_);
-		sol[i].obj_err = obj_err1_;
-		if(sol[i].obj_err < min_err)
-		{
-			min_err = sol[i].obj_err;
-			min_err_idx = i;
-		}
-	}
-
+	
+  // refine all poses 
+  for(unsigned int i=0; i<sol.size(); i++)
+  {
+    mat33_copy(options.initR,sol[i].R);
+    objpose(Rlu_, tlu_, it1_, obj_err1_, img_err1_, true, model, iprts, options);
+    mat33_copy(sol[i].PoseLu_R,Rlu_);
+    vec3_copy(sol[i].PoseLu_t,tlu_);
+    sol[i].obj_err = obj_err1_;
+  }
+  
+  //  from all solutions get the best 
+  int min_err_idx = (-1);
+  real_t min_err = MAX_FLOAT;
+  for(unsigned int i=0; i<sol.size(); i++)
+    if(sol[i].obj_err < min_err)
+    {
+      min_err = sol[i].obj_err;
+      min_err_idx = i;
+    }
+  
+  // Copy the best solution !!!
 	if(min_err_idx >= 0)
 	{
 		mat33_copy(R,sol[min_err_idx].PoseLu_R);
@@ -791,6 +986,44 @@
 		vec3_clear(t);
 		err = MAX_FLOAT;
 	}
+//  from all solutions get the 2nd Best one !!
+  int min_err_idx2 = min_err_idx;
+  min_err = MAX_FLOAT;
+  
+  for(unsigned int i=0; i<sol.size(); i++)
+    if(sol[i].obj_err < min_err  && (min_err_idx2!=i) )
+    {
+      min_err = sol[i].obj_err;
+      min_err_idx = i;
+    }
+  
+  // Copy the 2nd best solution !!!
+  if(min_err_idx >= 0)
+  {
+    mat33_copy(Rb,sol[min_err_idx].PoseLu_R);
+    vec3_copy(tb,sol[min_err_idx].PoseLu_t);
+    errb = sol[min_err_idx].obj_err;
+  }
+  else
+  {
+    mat33_clear(R);
+    vec3_clear(t);
+    err = MAX_FLOAT;
+  }
+  
+}
+
+// =====================================================================================
+void robust_pose(real_t &err, mat33_t &R, vec3_t &t,
+		 const vec3_array &_model, const vec3_array &_iprts,
+		 const options_t _options)
+{
+  real_t  errb;
+  vec3_t  tb;
+  mat33_t Rb;    
+  
+  // just call the function above 
+  robust_pose(err,R,t, errb,Rb,tb, _model,_iprts,_options);
 }
 
 // ----------------------------------------
diff -Nu librpp/rpp.h librpp.new/rpp.h
--- librpp/rpp.h	2009-09-26 14:43:46.578817323 +0200
+++ librpp.new/rpp.h	2008-01-30 21:36:40.000000000 +0100
@@ -36,7 +36,7 @@
  * ========================================================================
  ** @author   Thomas Pintaric
  *
- * $Id: rpp.h 162 2006-04-19 21:28:10Z grabner $
+ * $Id: rpp.h 199 2006-11-02 13:52:52Z schall $
  * @file
  * ======================================================================== */
 
@@ -73,6 +73,9 @@
 void robust_pose(real_t &err, mat33_t &R, vec3_t &t,
 				 const vec3_array &_model, const vec3_array &_iprts,
 				 const options_t _options);
+void robust_pose(real_t &err ,mat33_t & R ,vec3_t & t, 
+		 real_t &errb,mat33_t & Rb,vec3_t & tb,
+		 const vec3_array & model,const vec3_array & iprts,const options_t options);				 
 // ------------------------------------------------------------------------------------------
 } // namespace rpp
 
diff -Nu librpp/rpp_quintic.cpp librpp.new/rpp_quintic.cpp
--- librpp/rpp_quintic.cpp	2009-09-26 23:39:40.558818487 +0200
+++ librpp.new/rpp_quintic.cpp	2008-01-30 21:36:40.000000000 +0100
@@ -31,11 +31,14 @@
  * ========================================================================
  ** @author   Thomas Pintaric
  *
- * $Id: rpp_quintic.cpp 162 2006-04-19 21:28:10Z grabner $
+ * $Id: rpp_quintic.cpp 198 2006-11-01 16:00:04Z daniel $
  * @file
  * ======================================================================== */
 
 
+#include <ARToolKitPlus/OS.h>
+
+
 #ifndef _NO_LIBRPP_
 
 
diff -Nu librpp/rpp_svd.cpp librpp.new/rpp_svd.cpp
--- librpp/rpp_svd.cpp	2009-09-26 23:39:40.562817878 +0200
+++ librpp.new/rpp_svd.cpp	2008-01-30 21:36:40.000000000 +0100
@@ -33,6 +33,8 @@
  * ======================================================================== */
 
 
+#include <ARToolKitPlus/OS.h>
+
 #ifndef _NO_LIBRPP_
 
 
diff -Nu librpp/rpp_types.h librpp.new/rpp_types.h
--- librpp/rpp_types.h	2009-09-26 14:43:46.562835472 +0200
+++ librpp.new/rpp_types.h	2008-01-30 21:36:40.000000000 +0100
@@ -36,7 +36,7 @@
  * ========================================================================
  ** @author   Thomas Pintaric
  *
- * $Id: rpp_types.h 162 2006-04-19 21:28:10Z grabner $
+ * $Id: rpp_types.h 180 2006-08-20 16:57:22Z daniel $
  * @file
  * ======================================================================== */
 
@@ -45,9 +45,15 @@
 #ifndef __RPP_TYPES_H__
 #define __RPP_TYPES_H__
 
-#include <vector>
+#include <ARToolKitPlus/extra/stl_vector.h>
 #include "rpp_const.h"
 
+
+typedef double rpp_float;
+typedef double rpp_vec[3];
+typedef double rpp_mat[3][3];
+
+
 //
 // _USE_CUSTOMFLOAT_ is defined in rpp_const.h
 //
@@ -68,18 +74,22 @@
 //
 typedef real_t vec3[3];
 struct vec3_t { vec3 v; };
-typedef std::vector<vec3_t> vec3_array;
-typedef std::vector<vec3_t>::iterator vec3_array_iter;
-typedef std::vector<vec3_t>::const_iterator vec3_array_const_iter;
-
 typedef real_t mat33[3][3];
 struct mat33_t { mat33 m; };
-typedef std::vector<mat33_t> mat33_array;
-typedef std::vector<mat33_t>::iterator mat33_array_iter;
-typedef std::vector<mat33_t>::const_iterator mat33_array_const_iter;
-
-typedef std::vector<real_t> scalar_array;
 
+#ifndef __SYMBIAN32__
+	typedef std::vector<vec3_t> vec3_array;
+	typedef std::vector<vec3_t>::iterator vec3_array_iter;
+	typedef std::vector<vec3_t>::const_iterator vec3_array_const_iter;
+	typedef std::vector<mat33_t> mat33_array;
+	typedef std::vector<mat33_t>::iterator mat33_array_iter;
+	typedef std::vector<mat33_t>::const_iterator mat33_array_const_iter;
+	typedef std::vector<real_t> scalar_array;
+#else
+	class vec3_array;
+	class mat33_array;
+	class scalar_array;
+#endif //__SYMBIAN32__
 
 
 struct pose_t
@@ -92,7 +102,11 @@
 	real_t  obj_err;
 };
 
-typedef std::vector<pose_t> pose_vec;
+#ifndef __SYMBIAN32__
+  typedef std::vector<pose_t> pose_vec;
+#else
+  class pose_vec;
+#endif
 
 struct quat_t
 {
diff -Nu librpp/rpp_vecmat.cpp librpp.new/rpp_vecmat.cpp
--- librpp/rpp_vecmat.cpp	2009-09-26 23:39:40.562817878 +0200
+++ librpp.new/rpp_vecmat.cpp	2008-01-30 21:36:40.000000000 +0100
@@ -33,19 +33,26 @@
  * ========================================================================
  ** @author   Thomas Pintaric
  *
- * $Id: rpp_vecmat.cpp 162 2006-04-19 21:28:10Z grabner $
+ * $Id: rpp_vecmat.cpp 199 2006-11-02 13:52:52Z schall $
  * @file
  * ======================================================================== */
 
+#include <ARToolKitPlus/OS.h>
 
 #ifndef _NO_LIBRPP_
 
 
-#include "rpp_vecmat.h"
-#include <cmath>
-#include <cassert>
-#include <cstdlib>
+#include "rpp_vecmat.h"
+#include "math.h"
+#include "assert.h"
+
+#ifndef __APPLE__
+#include <malloc.h>
+#include <stdio.h>
+#else
 #include <cstdio>
+#include <malloc/malloc.h>
+#endif
 
 
 namespace rpp {
@@ -754,6 +761,31 @@
 	free_double_pptr(&m_ptr);
 	free_double_pptr(&v_ptr);
 	free_double_ptr(&q_ptr);
+	
+	// WE need to sort the diagonal values of the result !!!
+	// this is neccessery for the absolute orientation algo !!!
+	// Biggest value first !!
+	
+	bool sorted = false;
+	while( !sorted ){
+	  
+	  sorted = true;
+	  for(unsigned int i=1;i<3;i++)
+	    if( s.m[i-1][i-1] < s.m[i][i] ){
+	      sorted = false;   
+	      // change i mit i-1 !!!
+	      
+	      // in the S
+	      real_t t =  s.m[i-1][i-1] ;  s.m[i-1][i-1]  =  s.m[i][i]; s.m[i][i]=t;
+	      
+	      for(unsigned j=0;j<3;j++){
+		// in U 
+		t = u.m[j][i-1];  u.m[j][i-1] =  u.m[j][i];  u.m[j][i] = t;
+		// and in V
+		t = v.m[j][i-1];  v.m[j][i-1] =  v.m[j][i];  v.m[j][i] = t;
+	      }
+	    }
+	}
 }
 
 void quat_mult(quat_t &q, const real_t s)
diff -Nu librpp/rpp_vecmat.h librpp.new/rpp_vecmat.h
--- librpp/rpp_vecmat.h	2009-09-26 14:43:46.570817213 +0200
+++ librpp.new/rpp_vecmat.h	2008-01-30 21:36:40.000000000 +0100
@@ -33,7 +33,7 @@
  * ========================================================================
  ** @author   Thomas Pintaric
  *
- * $Id: rpp_vecmat.h 162 2006-04-19 21:28:10Z grabner $
+ * $Id: rpp_vecmat.h 199 2006-11-02 13:52:52Z schall $
  * @file
  * ======================================================================== */
 
@@ -44,6 +44,8 @@
 #include "rpp_const.h"
 #include "rpp_types.h"
 
+//#include <vector> 
+
 namespace rpp {
 
 real_t _sin(real_t a);
@@ -124,6 +126,7 @@
 void normRv(vec3_t &n, const vec3_t &v);
 void normRv(vec3_array &normR_v, const vec3_array &v);
 
+int solveBiCubic(std::vector<double> &k1, double a, double b, double c, double d, double e);
 int solve_polynomial(scalar_array &sol, const scalar_array &coefficients);
 void scalar_array_pow(scalar_array &sa, const real_t f);
 void scalar_array_negate(scalar_array &sa);
