00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include "phyuniversal.h"
00021 #include "private/phyjointprivate.h"
00022 #include "private/phyobjectprivate.h"
00023 #include "private/worldprivate.h"
00024
00025 namespace farsa {
00026
00027 PhyUniversal::PhyUniversal( const wVector& firstAxis, const wVector& secondAxis, const wVector& centre, PhyObject* parent, PhyObject* child, bool cp )
00028 : PhyJoint( parent, child ) {
00029 firstDof = new PhyDOF( this, firstAxis, centre, false );
00030 dofsv << firstDof;
00031 secondDof = new PhyDOF( this, secondAxis, centre, false );
00032 dofsv << secondDof;
00033 dofv = 2;
00034
00035
00036 localMatrixParent.x_ax = secondAxis;
00037 localMatrixParent.y_ax = firstAxis * secondAxis;
00038 localMatrixParent.z_ax = firstAxis;
00039 localMatrixParent.w_pos = centre;
00040 localMatrixParent.sanitifize();
00041
00042
00043 if ( parent ) {
00044 globalMatrixParent = localMatrixParent * parent->matrix();
00045 } else {
00046 globalMatrixParent = localMatrixParent;
00047 }
00048 localMatrixChild = globalMatrixParent * child->matrix().inverse();
00049 if ( parent ) {
00050 globalMatrixParent = localMatrixParent * parent->matrix();
00051 } else {
00052 globalMatrixParent = localMatrixParent;
00053 }
00054
00055 firstDof->setAxis( globalMatrixParent.z_ax );
00056 firstDof->setXAxis( globalMatrixParent.x_ax );
00057 firstDof->setYAxis( globalMatrixParent.y_ax );
00058 firstDof->setCentre( globalMatrixParent.w_pos );
00059
00060 globalMatrixChild = localMatrixChild * child->matrix();
00061 secondDof->setAxis( globalMatrixParent.x_ax );
00062 secondDof->setXAxis( globalMatrixParent.z_ax );
00063 secondDof->setYAxis( globalMatrixParent.y_ax );
00064 secondDof->setCentre( globalMatrixParent.w_pos );
00065
00066 if ( !localMatrixParent.sanityCheck() ) {
00067 qWarning( "Sanity Check Failed" );
00068 }
00069 if ( !localMatrixChild.sanityCheck() ) {
00070 qWarning( "Sanity Check Failed" );
00071 }
00072
00073 if ( cp ) {
00074 createPrivateJoint();
00075 }
00076 }
00077
00078 wVector PhyUniversal::centre() {
00079
00080 if ( parent() ) {
00081 globalMatrixParent = localMatrixParent * parent()->matrix();
00082 }
00083 return globalMatrixParent.w_pos;
00084 };
00085
00086 void PhyUniversal::createPrivateJoint() {
00087 #ifdef WORLDSIM_USE_NEWTON
00088 priv->joint = NewtonConstraintCreateUserJoint( worldpriv->world, 6, PhyJointPrivate::userBilateralHandler, 0, priv->child, priv->parent );
00089 NewtonJointSetUserData( priv->joint, this );
00090 #endif
00091 }
00092
00093 void PhyUniversal::updateJointInfo() {
00094
00095
00096
00097 if ( parent() ) {
00098 globalMatrixParent = localMatrixParent * parent()->matrix();
00099 }
00100 globalMatrixChild = localMatrixChild * child()->matrix();
00101
00102 if ( !globalMatrixParent.sanityCheck() ) {
00103 qWarning( "Sanity Check Failed" );
00104 }
00105 if ( !globalMatrixChild.sanityCheck() ) {
00106 qWarning( "Sanity Check Failed" );
00107 }
00108
00109 firstDof->setAxis( globalMatrixParent.z_ax );
00110 firstDof->setXAxis( globalMatrixParent.x_ax );
00111 firstDof->setYAxis( globalMatrixParent.y_ax );
00112 firstDof->setCentre( globalMatrixParent.w_pos );
00113
00114
00115
00116 real sinAngle = (globalMatrixParent.x_ax * globalMatrixChild.x_ax) % globalMatrixChild.z_ax;
00117 real cosAngle = globalMatrixChild.x_ax % globalMatrixParent.x_ax;
00118 real angle = atan2( sinAngle, cosAngle );
00119 firstDof->setPosition( angle );
00120
00121
00122 wMatrix tmp = wMatrix::identity();
00123 tmp.x_ax = globalMatrixParent.z_ax;
00124 tmp.y_ax = globalMatrixParent.y_ax;
00125 tmp.z_ax = globalMatrixParent.x_ax;
00126 tmp = tmp * wMatrix( wQuaternion( tmp.x_ax, angle ), wVector(0,0,0) );
00127 tmp.sanitifize();
00128 secondDof->setAxis( tmp.z_ax );
00129 secondDof->setXAxis( tmp.x_ax );
00130 secondDof->setYAxis( tmp.y_ax );
00131 secondDof->setCentre( globalMatrixParent.w_pos );
00132 }
00133
00134 void PhyUniversal::updateJoint( real timestep ) {
00135
00136
00137
00138 if ( parent() ) {
00139 globalMatrixParent = localMatrixParent * parent()->matrix();
00140 }
00141 globalMatrixChild = localMatrixChild * child()->matrix();
00142
00143 if ( !globalMatrixParent.sanityCheck() ) {
00144 qWarning( "Sanity Check Failed" );
00145 }
00146 if ( !globalMatrixChild.sanityCheck() ) {
00147 qWarning( "Sanity Check Failed" );
00148 }
00149
00150 firstDof->setAxis( globalMatrixParent.z_ax );
00151 firstDof->setXAxis( globalMatrixParent.x_ax );
00152 firstDof->setYAxis( globalMatrixParent.y_ax );
00153 firstDof->setCentre( globalMatrixParent.w_pos );
00154
00155
00156
00157
00158 #ifdef WORLDSIM_USE_NEWTON
00159
00160 const wVector& dir0 = globalMatrixChild.z_ax;
00161
00162 const wVector& dir1 = globalMatrixParent.x_ax;
00163
00164
00165 wVector dir2 = ( dir0 * dir1 );
00166 dir2.normalize();
00167 wVector dir3( dir2 * dir0 );
00168 dir3.normalize();
00169
00170 const wVector& p0 = globalMatrixChild.w_pos;
00171 const wVector& p1 = globalMatrixParent.w_pos;
00172
00173 real len = 100.0;
00174 wVector q0( p0 + dir3.scale(len) );
00175 wVector q1( p1 + dir1.scale(len) );
00176
00177
00178 NewtonUserJointAddLinearRow( priv->joint, &p0[0], &p1[0], &dir0[0] );
00179 NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
00180 NewtonUserJointAddLinearRow( priv->joint, &p0[0], &p1[0], &dir1[0] );
00181 NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
00182 NewtonUserJointAddLinearRow( priv->joint, &p0[0], &p1[0], &dir2[0] );
00183 NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
00184
00185
00186 NewtonUserJointAddLinearRow( priv->joint, &q0[0], &q1[0], &dir0[0] );
00187 NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
00188
00189
00190
00191
00192
00193 real force = fabs( firstDof->appliedForce() );
00194 real accel = 2.0*firstDof->appliedForce();
00195
00196
00197
00198 real sinAngle = (globalMatrixParent.x_ax * globalMatrixChild.x_ax) % globalMatrixChild.z_ax;
00199 real cosAngle = globalMatrixChild.x_ax % globalMatrixParent.x_ax;
00200 real angle = atan2( sinAngle, cosAngle );
00201 real vel = (angle - firstDof->position()) / timestep;
00202 firstDof->setPosition( angle );
00203 firstDof->setVelocity( vel );
00204 if ( firstDof->isLimited() ) {
00205 real lolimit;
00206 real hilimit;
00207 firstDof->limits( lolimit, hilimit );
00208 if ( angle < lolimit ) {
00209 real relAngle = lolimit - angle;
00210 NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.z_ax[0] );
00211 NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
00212
00213 NewtonUserJointSetRowMinimumFriction( priv->joint, 0.0 );
00214 } else if ( angle > hilimit ) {
00215 real relAngle = hilimit - angle;
00216 NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.z_ax[0] );
00217 NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
00218
00219 NewtonUserJointSetRowMaximumFriction( priv->joint, 0.0 );
00220 } else {
00221 NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.z_ax[0] );
00222 NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
00223 NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
00224 NewtonUserJointSetRowAcceleration( priv->joint, accel );
00225 NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
00226 }
00227 } else {
00228 NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.z_ax[0] );
00229 NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
00230 NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
00231 NewtonUserJointSetRowAcceleration( priv->joint, accel );
00232 NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
00233 }
00234
00235
00236 wMatrix tmp = wMatrix::identity();
00237 tmp.x_ax = globalMatrixParent.z_ax;
00238 tmp.y_ax = globalMatrixParent.y_ax;
00239 tmp.z_ax = globalMatrixParent.x_ax;
00240 tmp = tmp * wMatrix( wQuaternion( tmp.x_ax, angle ), wVector(0,0,0) );
00241 tmp.sanitifize();
00242 secondDof->setAxis( tmp.z_ax );
00243 secondDof->setXAxis( tmp.x_ax );
00244 secondDof->setYAxis( tmp.y_ax );
00245 secondDof->setCentre( globalMatrixParent.w_pos );
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255 force = fabs( secondDof->appliedForce() );
00256 accel = 2.0*secondDof->appliedForce();
00257
00258
00259
00260
00261 sinAngle = (globalMatrixParent.z_ax * globalMatrixChild.z_ax) % globalMatrixChild.x_ax;
00262 cosAngle = globalMatrixChild.z_ax % globalMatrixParent.z_ax;
00263 angle = atan2( sinAngle, cosAngle );
00264 vel = (angle - secondDof->position()) / timestep;
00265 secondDof->setPosition( angle );
00266 secondDof->setVelocity( vel );
00267 if ( secondDof->isLimited() ) {
00268 real lolimit;
00269 real hilimit;
00270 secondDof->limits( lolimit, hilimit );
00271 if ( angle < lolimit ) {
00272 real relAngle = lolimit - angle;
00273 NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.x_ax[0] );
00274 NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
00275
00276 NewtonUserJointSetRowMinimumFriction( priv->joint, 0.0 );
00277 } else if ( angle > hilimit ) {
00278 real relAngle = hilimit - angle;
00279 NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.x_ax[0] );
00280 NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
00281
00282 NewtonUserJointSetRowMaximumFriction( priv->joint, 0.0 );
00283 } else {
00284 NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.x_ax[0] );
00285 NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
00286 NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
00287 NewtonUserJointSetRowAcceleration( priv->joint, accel );
00288 NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
00289 }
00290 } else {
00291 NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.x_ax[0] );
00292 NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
00293 NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
00294 NewtonUserJointSetRowAcceleration( priv->joint, accel );
00295 NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
00296 }
00297
00298 #endif
00299
00300 }
00301
00302 }