[Opal-commits] opal/src Matrix44r.h,1.34,1.35 ServoMotor.cpp,1.17,1.18

[Olex <ole...@us...>]

Update of /cvsroot/opal/opal/src
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv3634/src

Modified Files:
	Matrix44r.h ServoMotor.cpp 
Log Message:
Fixed ServoMotor's calculation of the angular velocity needed to bring the joint to the desired angle. Cleaned up some warning-generating code in Matrix44r::getQuaternion()


Index: ServoMotor.cpp
===================================================================
RCS file: /cvsroot/opal/opal/src/ServoMotor.cpp,v
retrieving revision 1.17
retrieving revision 1.18
diff -C2 -d -r1.17 -r1.18
*** ServoMotor.cpp	4 Jun 2005 20:20:21 -0000	1.17
--- ServoMotor.cpp	26 Jun 2005 16:57:35 -0000	1.18
***************
*** 133,136 ****
--- 133,140 ----
  					real velocity = mData.desiredAngle - 
  						mData.joint->getAngle(mData.jointAxisNum);
+ 					if ( velocity > 180.0 )
+ 						velocity = -360 + velocity;
+ 					if ( velocity < -180.0 )
+ 						velocity = 360 + velocity;
  					mData.joint->internal_setDesiredVel(mData.jointAxisNum, 
  						mData.restoreSpeed * velocity);

Index: Matrix44r.h
===================================================================
RCS file: /cvsroot/opal/opal/src/Matrix44r.h,v
retrieving revision 1.34
retrieving revision 1.35
diff -C2 -d -r1.34 -r1.35
*** Matrix44r.h	27 Apr 2005 02:28:18 -0000	1.34
--- Matrix44r.h	26 Jun 2005 16:57:34 -0000	1.35
***************
*** 287,292 ****
  				if((*this)(0,0) > (*this)(1,1) && (*this)(0,0) > (*this)(2,2))
  				{
! 					real s = 2.0f * sqrtf(1.0f + (*this)(0,0) - (*this)(1,1) - 
! 						(*this)(2,2));
  					q[1] = 0.25f * s;
  					q[2] = ((*this)(0,1) + (*this)(1,0)) / s;
--- 287,292 ----
  				if((*this)(0,0) > (*this)(1,1) && (*this)(0,0) > (*this)(2,2))
  				{
! 					real s = 2.0 * sqrt(static_cast<real>(1.0 + (*this)(0,0) - (*this)(1,1) - 
! 						(*this)(2,2)));
  					q[1] = 0.25f * s;
  					q[2] = ((*this)(0,1) + (*this)(1,0)) / s;
***************
*** 296,301 ****
  				else if ((*this)(1,1) > (*this)(2,2))
  				{
! 					real s = 2.0f * sqrtf(1.0f + (*this)(1,1) - (*this)(0,0) - 
! 						(*this)(2,2));
  					q[1] = ((*this)(0,1) + (*this)(1,0)) / s;
  					q[2] = 0.25f * s;
--- 296,301 ----
  				else if ((*this)(1,1) > (*this)(2,2))
  				{
! 					real s = 2.0 * sqrt(static_cast<real>(1.0 + (*this)(1,1) - (*this)(0,0) - 
! 						(*this)(2,2)));
  					q[1] = ((*this)(0,1) + (*this)(1,0)) / s;
  					q[2] = 0.25f * s;
***************
*** 305,310 ****
  				else
  				{
! 					real s = 2.0f * sqrtf(1.0f + (*this)(2,2) - (*this)(0,0) - 
! 						(*this)(1,1));
  					q[1] = ((*this)(0,2) + (*this)(2,0)) / s;
  					q[2] = ((*this)(1,2) + (*this)(2,1)) / s;
--- 305,310 ----
  				else
  				{
! 					real s = 2.0 * sqrt(static_cast<real>(1.0 + (*this)(2,2) - (*this)(0,0) - 
! 						(*this)(1,1)));
  					q[1] = ((*this)(0,2) + (*this)(2,0)) / s;
  					q[2] = ((*this)(1,2) + (*this)(2,1)) / s;