char * gLatLon2RaDec( double gLat, double gLon, double * pRa, double *pDec)
/* * * * * * * * * * * * * * * * */
/* gLatLonToRaDec() coverts input gLat, gLon (radians) to J2000 Ra, Dec (Rad)*/
/* returns NULL on OK */
/* CALCULATE RA DEC COORDINATES (1950) (use different constants for J2000) */
/* -1 */
/* dec = sin ( cos(b)cos(27.4)sin(l-33.) + sin(b)sin(27.4) ) */
/* */
/* -1 cos(b) cos(l-33) */
/* ra = tan ( ----------------------------------------- ) */
/* sin(b)cos(27.4) - cos(b)sin(27.4)sin(l-33) */
/* DEC = ASIN (SIN(GLAT)*SIN(DECGP) + COS(GLAT)*COS(DECGP)* */
/* * COS(GLON-LONCP)) */
/* RA = RAGP + ATAN2 (COS(GLAT)*SIN(LONCP-GLON), SIN(GLAT)* */
/* * COS(DECGP) - COS(GLAT)*SIN(DECGP)*COS(GLON-LONCP)) */
/* * * * * * * * * * * * * * * * */
{ double sl, cl, sb, cb, sd, l = gLon - LONCELESTIALPOLE,
b = gLat, ra, dec;
sl = sin(l);
cl = cos(l);
sb = sin(b);
cb = cos(b);
sd = (sb*sc) + (cb*cc*cl); /*sin(dec)*/
dec = asin(sd);
if (dec >= TWOPI) /*make range 90 to -90*/
dec -= TWOPI;
if (dec >= PI) /*make range 90 to -90*/
dec = TWOPI - dec;
if (dec >= PIOVR2)
dec = PI - dec;
if (1. - fabs(sd) < EPSILON) /* if near the pole */
ra = 0.;
else
ra = (atan2(-sl*cb,(sb*cc)-(cb*sc*cl))) + POLERA;
angleLimit( &ra);
*pRa = ra;
*pDec = dec;
return(NULL);
} /* end of gLatLon2RaDec() */
char * raDec2gLatLon( double ra, double dec, double * gLat, double * gLon)
/* * * * * * * * * * * * * * * * */
/* reDec2gLatLon() converts the ra and dec (J2000) to galactic latitude and */
/* longitude. Returns NULL ON OK, else error message */
/* CALCULATE GALACTIC COORDINATES */
/* -1 */
/* b = sin ( cos(dec)cos(27.4)cos(ra-192.25) + sin(dec)sin(27.4) ) */
/* */
/* -1 sin(dec) - (sin(b) sin(27.4)) */
/* l = tan ( -------------------------------- ) */
/* cos(dec)sin(ra-192.25)cos(27.4) */
/* AIPS: 15APR98 */
/* GLAT = ASIN (SIN(DEC)*SIN(DECGP) + COS(DEC)*COS(DECGP)* */
/* * COS(RA-RAGP)) */
/* GLON = LONCP + ATAN2 (COS(DEC)*SIN(RAGP-RA), SIN(DEC)* */
/* * COS(DECGP) - COS(DEC)*SIN(DECGP)*COS(RA-RAGP)) */
/* * * * * * * * * * * * * * * * */
{ double sr, cr, sd, cd, sb;
ra -= POLERA; /* RA relative to galactic pole */
sr = sin(ra);
cr = cos(ra);
sd = sin(dec);
cd = cos(dec);
sb = (cd*cc*cr) + (sd*sc); /* sin(b)*/
*gLat = asin(sb); /* galactic latitude*/
if (*gLat >= TWOPI) /* keep range 90 to -90*/
*gLat = *gLat - TWOPI;
if (*gLat > PI) /* make range 90 to -90*/
*gLat = TWOPI - *gLat;
if (*gLat > PIOVR2)
*gLat = PI - *gLat;
if (1. - fabs(sb) < EPSILON) /* if near the pole */
*gLon = 0.; /* set galactic long */
else
*gLon = (atan2(-sr*cd,(sd*cc)-(cd*sc*cr))) + LONCELESTIALPOLE;
angleLimit( gLon);
return(NULL);
} /* end of raDec2gLatLon() */
enum TrajectoryFollower::FOLLOWER_STATUS TrajectoryFollower::traverseTrajectory(Eigen::Vector2d &motionCmd, const base::Pose &robotPose)
{
motionCmd(0) = 0.0;
motionCmd(1) = 0.0;
if(!hasTrajectory)
return REACHED_TRAJECTORY_END;
base::Trajectory &trajectory(currentTrajectory);
if(newTrajectory)
{
newTrajectory = false;
para = trajectory.spline.findOneClosestPoint(robotPose.position, 0.001);
}
pose.position = robotPose.position;
pose.heading = robotPose.getYaw();
if ( para < trajectory.spline.getEndParam() )
{
double dir = 1.0;
if(!trajectory.driveForward())
{
pose.heading = angleLimit(pose.heading+M_PI);
dir = -1.0;
}
double fwLenght = 0;
if(controllerType == 0)
{
fwLenght = dir * forwardLength + gpsCenterofRotationOffset;
}
else
{
//fwLenght = dir * gpsCenterofRotationOffset;
}
pose.position += AngleAxisd(pose.heading, Vector3d::UnitZ()) * Vector3d(fwLenght, 0, 0);
Eigen::Vector3d vError = trajectory.spline.poseError(pose.position, pose.heading, para);
para = vError(2);
//distance error
error.d = vError(0);
//heading error
error.theta_e = angleLimit(vError(1) + addPoseErrorY);
//spline parameter for traget point on spline
error.param = vError(2);
curvePoint.pose.position = trajectory.spline.getPoint(para);
curvePoint.pose.heading = trajectory.spline.getHeading(para);
curvePoint.param = para;
//disable this test for testing, as it seems to be not needed
bInitStable = true;
if(!bInitStable)
{
switch(controllerType)
{
case 0:
bInitStable = oTrajController_nO.checkInitialStability(error.d, error.theta_e, trajectory.spline.getCurvatureMax());
bInitStable = true;
break;
case 1:
bInitStable = oTrajController_P.checkInitialStability(error.d, error.theta_e, trajectory.spline.getCurvature(para), trajectory.spline.getCurvatureMax());
break;
case 2:
bInitStable = oTrajController_PI.checkInitialStability(error.d, error.theta_e, trajectory.spline.getCurvature(para), trajectory.spline.getCurvatureMax());
break;
default:
throw std::runtime_error("Got bad controllerType value");
}
if (!bInitStable)
{
LOG_DEBUG_S << "Trajectory controller: failed initial stability test";
return INITIAL_STABILITY_FAILED;
}
}
double vel = currentTrajectory.speed;
switch(controllerType)
{
case 0:
motionCmd = oTrajController_nO.update(vel, error.d, error.theta_e);
break;
case 1:
motionCmd = oTrajController_P.update(vel, error.d, error.theta_e, trajectory.spline.getCurvature(para), trajectory.spline.getVariationOfCurvature(para));
break;
case 2:
motionCmd = oTrajController_PI.update(vel, error.d, error.theta_e, trajectory.spline.getCurvature(para), trajectory.spline.getVariationOfCurvature(para));
break;
default:
throw std::runtime_error("Got bad controllerType value");
}
LOG_DEBUG_S << "Mc: " << motionCmd(0) << " " << motionCmd(1)
<< " error: d " << error.d << " theta " << error.theta_e << " PI";
}
else
{
if(status != REACHED_TRAJECTORY_END)
{
LOG_INFO_S << "Reached end of trajectory" << std::endl;
status = REACHED_TRAJECTORY_END;
}
return REACHED_TRAJECTORY_END;
}
if(status != RUNNING)
{
LOG_INFO_S << "Started to follow trajectory" << std::endl;
status = RUNNING;
}
return RUNNING;
}
int runCTest( const std::string& testName )
{
const std::string fileName( "testconstraint.osg" );
// Create two rigid bodies for testing.
osg::ref_ptr< osg::Group > root = new osg::Group;
osg::Node* node = osgDB::readNodeFile( "tetra.osg" );
if( node == NULL )
ERROR("Init:","Can't load model data file.");
osg::Matrix aXform = osg::Matrix::translate( 4., 2., 0. );
osgwTools::AbsoluteModelTransform* amt = new osgwTools::AbsoluteModelTransform;
amt->setDataVariance( osg::Object::DYNAMIC );
amt->addChild( node );
root->addChild( amt );
osg::ref_ptr< osgbDynamics::CreationRecord > cr = new osgbDynamics::CreationRecord;
cr->_sceneGraph = amt;
cr->_shapeType = BOX_SHAPE_PROXYTYPE;
cr->_mass = .5;
cr->_parentTransform = aXform;
btRigidBody* rbA = osgbDynamics::createRigidBody( cr.get() );
node = osgDB::readNodeFile( "block.osg" );
if( node == NULL )
ERROR("Init:","Can't load model data file.");
osg::Matrix bXform = osg::Matrix::identity();
amt = new osgwTools::AbsoluteModelTransform;
amt->setDataVariance( osg::Object::DYNAMIC );
amt->addChild( node );
root->addChild( amt );
cr = new osgbDynamics::CreationRecord;
cr->_sceneGraph = amt;
cr->_shapeType = BOX_SHAPE_PROXYTYPE;
cr->_mass = 4.;
cr->_parentTransform = bXform;
btRigidBody* rbB = osgbDynamics::createRigidBody( cr.get() );
//
// SliderConstraint
if( testName == std::string( "Slider" ) )
{
osg::Vec3 axis( 0., 0., 1. );
osg::Vec2 limits( -4., 4. );
osg::ref_ptr< osgbDynamics::SliderConstraint > cons = new osgbDynamics::SliderConstraint(
rbA, aXform, rbB, bXform, axis, limits );
if( cons->getAsBtSlider() == NULL )
ERROR(testName,"won't typecast as btSliderConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::SliderConstraint > cons2 = dynamic_cast<
osgbDynamics::SliderConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// TwistSliderConstraint
if( testName == std::string( "TwistSlider" ) )
{
osg::Vec3 axis( 0., 0., 1. );
osg::Vec3 point( 1., 2., 3. );
osg::Vec2 linLimits( -4., 4. );
osg::Vec2 angLimits( -1., 2. );
osg::ref_ptr< osgbDynamics::TwistSliderConstraint > cons = new osgbDynamics::TwistSliderConstraint(
rbA, aXform, rbB, bXform, axis, point, linLimits, angLimits );
if( cons->getAsBtSlider() == NULL )
ERROR(testName,"won't typecast as btSliderConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::TwistSliderConstraint > cons2 = dynamic_cast<
osgbDynamics::TwistSliderConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// BallAndSocketConstraint
if( testName == std::string( "BallAndSocket" ) )
{
osg::Vec3 point( -5., 5., 3. );
osg::ref_ptr< osgbDynamics::BallAndSocketConstraint > cons = new osgbDynamics::BallAndSocketConstraint(
rbA, aXform, rbB, bXform, point );
if( cons->getAsBtPoint2Point() == NULL )
ERROR(testName,"won't typecast as btPoint2PointConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::BallAndSocketConstraint > cons2 = dynamic_cast<
osgbDynamics::BallAndSocketConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// FixedConstraint
if( testName == std::string( "Fixed" ) )
{
osg::ref_ptr< osgbDynamics::FixedConstraint > cons = new osgbDynamics::FixedConstraint(
rbA, aXform, rbB, bXform );
if( cons->getAsBtGeneric6Dof() == NULL )
ERROR(testName,"won't typecast as btGeneric6DofConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::FixedConstraint > cons2 = dynamic_cast<
osgbDynamics::FixedConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// PlanarConstraint
if( testName == std::string( "Planar" ) )
{
osg::Vec2 loLimit( -2., -3. );
osg::Vec2 hiLimit( 1., 4. );
osg::Matrix orient( osg::Matrix::identity() );
osg::ref_ptr< osgbDynamics::PlanarConstraint > cons = new osgbDynamics::PlanarConstraint(
rbA, aXform, rbB, bXform, loLimit, hiLimit, orient );
if( cons->getAsBtGeneric6Dof() == NULL )
ERROR(testName,"won't typecast as btGeneric6DofConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::PlanarConstraint > cons2 = dynamic_cast<
osgbDynamics::PlanarConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// BoxConstraint
if( testName == std::string( "Box" ) )
{
osg::Vec3 loLimit( -2., -3., 0. );
osg::Vec3 hiLimit( 1., 4., 2. );
osg::Matrix orient( osg::Matrix::identity() );
osg::ref_ptr< osgbDynamics::BoxConstraint > cons = new osgbDynamics::BoxConstraint(
rbA, aXform, rbB, bXform, loLimit, hiLimit, orient );
if( cons->getAsBtGeneric6Dof() == NULL )
ERROR(testName,"won't typecast as btGeneric6DofConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::BoxConstraint > cons2 = dynamic_cast<
osgbDynamics::BoxConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// LinearSpringConstraint
if( testName == std::string( "LinearSpring" ) )
{
osg::ref_ptr< osgbDynamics::LinearSpringConstraint > cons = new osgbDynamics::LinearSpringConstraint(
rbA, aXform, rbB, bXform, osg::Vec3( 2., 1., 0. ) );
cons->setLimit( osg::Vec2( -2., 3. ) );
cons->setStiffness( 40.f );
cons->setDamping( .5f );
if( cons->getAsBtGeneric6DofSpring() == NULL )
ERROR(testName,"won't typecast as btGeneric6DofSpringConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::LinearSpringConstraint > cons2 = dynamic_cast<
osgbDynamics::LinearSpringConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// AngleSpringConstraint
if( testName == std::string( "AngleSpring" ) )
{
osg::ref_ptr< osgbDynamics::AngleSpringConstraint > cons = new osgbDynamics::AngleSpringConstraint(
rbA, aXform, rbB, bXform, osg::Vec3( 2., 1., 0. ), osg::Vec3( 5., 6., -7. ) );
cons->setLimit( osg::Vec2( -2., 1. ) );
cons->setStiffness( 50.f );
cons->setDamping( 0.f );
if( cons->getAsBtGeneric6DofSpring() == NULL )
ERROR(testName,"won't typecast as btGeneric6DofSpringConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::AngleSpringConstraint > cons2 = dynamic_cast<
osgbDynamics::AngleSpringConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// LinearAngleSpringConstraint
if( testName == std::string( "LinearAngleSpring" ) )
{
osg::ref_ptr< osgbDynamics::LinearAngleSpringConstraint > cons = new osgbDynamics::LinearAngleSpringConstraint(
rbA, aXform, rbB, bXform, osg::Vec3( 2., 1., 0. ), osg::Vec3( 5., 6., -7. ) );
cons->setLinearLimit( osg::Vec2( -2., 2. ) );
cons->setAngleLimit( osg::Vec2( -3., 3. ) );
cons->setLinearStiffness( 41.f );
cons->setLinearDamping( 1.f );
cons->setAngleStiffness( 42.f );
cons->setAngleDamping( 2.f );
if( cons->getAsBtGeneric6DofSpring() == NULL )
ERROR(testName,"won't typecast as btGeneric6DofSpringConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::LinearAngleSpringConstraint > cons2 = dynamic_cast<
osgbDynamics::LinearAngleSpringConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// HingeConstraint
if( testName == std::string( "Hinge" ) )
{
osg::Vec3 axis( -1., 0., 0. );
osg::Vec3 point( 4., 3., 2. );
osg::Vec2 limit( -2., 2. );
osg::ref_ptr< osgbDynamics::HingeConstraint > cons = new osgbDynamics::HingeConstraint(
rbA, aXform, rbB, bXform, axis, point, limit );
if( cons->getAsBtHinge() == NULL )
ERROR(testName,"won't typecast as btHingeConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::HingeConstraint > cons2 = dynamic_cast<
osgbDynamics::HingeConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// CardanConstraint
if( testName == std::string( "Cardan" ) )
{
osg::Vec3 axisA( -1., 0., 0. );
osg::Vec3 axisB( 0., 0., 1. );
osg::ref_ptr< osgbDynamics::CardanConstraint > cons = new osgbDynamics::CardanConstraint(
rbA, aXform, rbB, bXform, axisA, axisB );
if( cons->getAsBtUniversal() == NULL )
ERROR(testName,"won't typecast as btUniversalConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::CardanConstraint > cons2 = dynamic_cast<
osgbDynamics::CardanConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// RagdollConstraint
if( testName == std::string( "Ragdoll" ) )
{
osg::Vec3 point( 0., 1., 2. );
osg::Vec3 axis( 0., 1., 0. );
double angle = 2.;
osg::ref_ptr< osgbDynamics::RagdollConstraint > cons = new osgbDynamics::RagdollConstraint(
rbA, aXform, rbB, bXform, point, axis, angle );
if( cons->getAsBtConeTwist() == NULL )
ERROR(testName,"won't typecast as btConeTwistConstraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::RagdollConstraint > cons2 = dynamic_cast<
osgbDynamics::RagdollConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
//
// WheelSuspensionConstraint
if( testName == std::string( "WheelSuspension" ) )
{
osg::Vec3 springAxis( 0., 0., 1. );
osg::Vec3 axleAxis( 0., 1., 0. );
osg::Vec2 linearLimit( -2., 3. );
osg::Vec2 angleLimit( -1., 1. );
osg::Vec3 anchor( 0., 1., 2. );
osg::ref_ptr< osgbDynamics::WheelSuspensionConstraint > cons = new osgbDynamics::WheelSuspensionConstraint(
rbA, rbB, springAxis, axleAxis, linearLimit, angleLimit, anchor );
if( cons->getAsBtHinge2() == NULL )
ERROR(testName,"won't typecast as btHinge2Constraint.");
if( !( osgDB::writeObjectFile( *cons, fileName ) ) )
ERROR(testName,"writeObjectFile failed.");
osg::Object* obj = osgDB::readObjectFile( fileName );
if( obj == NULL )
ERROR(testName,"readObjectFile returned NULL.");
osg::ref_ptr< osgbDynamics::WheelSuspensionConstraint > cons2 = dynamic_cast<
osgbDynamics::WheelSuspensionConstraint* >( obj );
if( !( cons2.valid() ) )
ERROR(testName,"dynamic_cast after readObjectFile failed.");
if( *cons2 != *cons )
// Note matches can fail due to double precision roundoff.
// For testing, use only 1s and 0s in matrices.
ERROR(testName,"failed to match.");
return( 0 );
}
ERROR(testName,"unknown test name.");
}