本文整理汇总了C++中dMultiply0_331函数的典型用法代码示例。如果您正苦于以下问题:C++ dMultiply0_331函数的具体用法?C++ dMultiply0_331怎么用?C++ dMultiply0_331使用的例子?那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了dMultiply0_331函数的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: dMultiply0_331
void
dxJointHinge2::makeV1andV2()
{
if ( node[0].body )
{
// get axis 1 and 2 in global coords
dVector3 ax1, ax2, v;
dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
// don't do anything if the axis1 or axis2 vectors are zero or the same
if ((_dequal(ax1[0], 0.0) && _dequal(ax1[1], 0.0) && _dequal(ax1[2], 0.0)) ||
(_dequal(ax2[0], 0.0) && _dequal(ax2[1], 0.0) && _dequal(ax2[2], 0.0)) ||
(_dequal(ax1[0], ax2[0]) && _dequal(ax1[1], ax2[1]) && _dequal(ax1[2], ax2[2])))
return;
// modify axis 2 so it's perpendicular to axis 1
dReal k = dCalcVectorDot3( ax1, ax2 );
for ( int i = 0; i < 3; i++ ) ax2[i] -= k * ax1[i];
dNormalize3( ax2 );
// make v1 = modified axis2, v2 = axis1 x (modified axis2)
dCalcVectorCross3( v, ax1, ax2 );
dMultiply1_331( v1, node[0].body->posr.R, ax2 );
dMultiply1_331( v2, node[0].body->posr.R, v );
}
}
开发者ID:abyravan,项目名称:gazebo-2.2,代码行数:27,代码来源:hinge2.cpp
示例2: dMultiply0_331
void
dxJointAMotor::setEulerReferenceVectors()
{
if ( node[0].body && node[1].body )
{
dVector3 r; // axis[2] and axis[0] in global coordinates
dMultiply0_331( r, node[1].body->posr.R, axis[2] );
dMultiply1_331( reference1, node[0].body->posr.R, r );
dMultiply0_331( r, node[0].body->posr.R, axis[0] );
dMultiply1_331( reference2, node[1].body->posr.R, r );
}
else // jds
{
// else if (j->node[0].body) {
// dMultiply1_331 (j->reference1,j->node[0].body->posr.R,j->axis[2]);
// dMultiply0_331 (j->reference2,j->node[0].body->posr.R,j->axis[0]);
// We want to handle angular motors attached to passive geoms
dVector3 r; // axis[2] and axis[0] in global coordinates
r[0] = axis[2][0];
r[1] = axis[2][1];
r[2] = axis[2][2];
r[3] = axis[2][3];
dMultiply1_331( reference1, node[0].body->posr.R, r );
dMultiply0_331( r, node[0].body->posr.R, axis[0] );
reference2[0] += r[0];
reference2[1] += r[1];
reference2[2] += r[2];
reference2[3] += r[3];
}
}
开发者ID:JdeRobot,项目名称:ThirdParty,代码行数:32,代码来源:amotor.cpp
示例3: dMultiply0_331
void
dxJointHinge2::makeW1andW2()
{
if ( node[1].body )
{
// get axis 1 and 2 in global coords
dVector3 ax1, ax2, w;
dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
// don't do anything if the axis1 or axis2 vectors are zero or the same
if (( ax1[0] == 0 && ax1[1] == 0 && ax1[2] == 0 ) ||
( ax2[0] == 0 && ax2[1] == 0 && ax2[2] == 0 ) ||
( ax1[0] == ax2[0] && ax1[1] == ax2[1] && ax1[2] == ax2[2] ) ) return;
// modify axis 1 so it's perpendicular to axis 2
dReal k = dCalcVectorDot3( ax2, ax1 );
for ( int i = 0; i < 3; i++ ) ax1[i] -= k * ax2[i];
dNormalize3( ax1 );
// make w1 = modified axis1, w2 = axis2 x (modified axis1)
dCalcVectorCross3( w, ax2, ax1 );
dMultiply1_331( w1, node[1].body->posr.R, ax1 );
dMultiply1_331( w2, node[1].body->posr.R, w );
}
}
开发者ID:emperorstarfinder,项目名称:opensim-libs,代码行数:26,代码来源:hinge2.cpp
示例4: dJointGetAMotorAxis
void dJointGetAMotorAxis( dJointID j, int anum, dVector3 result )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dAASSERT( joint && anum >= 0 && anum < 3 );
checktype( joint, AMotor );
if ( anum < 0 ) anum = 0;
if ( anum > 2 ) anum = 2;
if ( joint->rel[anum] > 0 )
{
if ( joint->rel[anum] == 1 )
{
dMultiply0_331( result, joint->node[0].body->posr.R, joint->axis[anum] );
}
else
{
if ( joint->node[1].body ) // jds
{
dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis[anum] );
}
else
{
result[0] = joint->axis[anum][0];
result[1] = joint->axis[anum][1];
result[2] = joint->axis[anum][2];
result[3] = joint->axis[anum][3];
}
}
}
else
{
result[0] = joint->axis[anum][0];
result[1] = joint->axis[anum][1];
result[2] = joint->axis[anum][2];
}
}
开发者ID:JdeRobot,项目名称:ThirdParty,代码行数:35,代码来源:amotor.cpp
示例5: dJointGetAMotorAxis
void dJointGetAMotorAxis( dJointID j, int anum, dVector3 result )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dAASSERT( joint && anum >= 0 && anum < 3 );
checktype( joint, AMotor );
if ( anum < 0 ) anum = 0;
if ( anum > 2 ) anum = 2;
// If we're in Euler mode, joint->axis[1] doesn't
// have anything sensible in it. So don't just return
// that, find the actual effective axis.
// Likewise, the actual axis of rotation for the
// the other axes is different from what's stored.
if ( joint->mode == dAMotorEuler ) {
dVector3 axes[3];
joint->computeGlobalAxes(axes);
if (anum == 1) {
result[0]=axes[1][0];
result[1]=axes[1][1];
result[2]=axes[1][2];
} else if (anum == 0) {
// This won't be unit length in general,
// but it's what's used in getInfo2
// This may be why things freak out as
// the body-relative axes get close to each other.
dCalcVectorCross3( result, axes[1], axes[2] );
} else if (anum == 2) {
// Same problem as above.
dCalcVectorCross3( result, axes[0], axes[1] );
}
} else if ( joint->rel[anum] > 0 ) {
if ( joint->rel[anum] == 1 )
{
dMultiply0_331( result, joint->node[0].body->posr.R, joint->axis[anum] );
}
else
{
if ( joint->node[1].body ) // jds
{
dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis[anum] );
}
else
{
result[0] = joint->axis[anum][0];
result[1] = joint->axis[anum][1];
result[2] = joint->axis[anum][2];
result[3] = joint->axis[anum][3];
}
}
}
else
{
result[0] = joint->axis[anum][0];
result[1] = joint->axis[anum][1];
result[2] = joint->axis[anum][2];
}
}
开发者ID:JohnCrash,项目名称:ode,代码行数:57,代码来源:amotor.cpp
示例6: dMultiply0_331
// compute the 3 axes in global coordinates
void
dxJointAMotor::computeGlobalAxes( dVector3 ax[3] )
{
if ( mode == dAMotorEuler )
{
// special handling for euler mode
dMultiply0_331( ax[0], node[0].body->posr.R, axis[0] );
if ( node[1].body )
{
dMultiply0_331( ax[2], node[1].body->posr.R, axis[2] );
}
else
{
ax[2][0] = axis[2][0];
ax[2][1] = axis[2][1];
ax[2][2] = axis[2][2];
}
dCalcVectorCross3( ax[1], ax[2], ax[0] );
dNormalize3( ax[1] );
}
else
{
for ( int i = 0; i < num; i++ )
{
if ( rel[i] == 1 )
{
// relative to b1
dMultiply0_331( ax[i], node[0].body->posr.R, axis[i] );
}
else if ( rel[i] == 2 )
{
// relative to b2
if ( node[1].body ) // jds: don't assert, just ignore
{
dMultiply0_331( ax[i], node[1].body->posr.R, axis[i] );
}
else
{
// global - just copy it
ax[i][0] = axis[i][0];
ax[i][1] = axis[i][1];
ax[i][2] = axis[i][2];
}
}
else
{
// global - just copy it
ax[i][0] = axis[i][0];
ax[i][1] = axis[i][1];
ax[i][2] = axis[i][2];
}
}
}
}
开发者ID:JohnCrash,项目名称:ode,代码行数:55,代码来源:amotor.cpp
示例7: dJointGetPRPosition
dReal dJointGetPRPosition( dJointID j )
{
dxJointPR* joint = ( dxJointPR* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PR );
dVector3 q;
// get the offset in global coordinates
dMultiply0_331( q, joint->node[0].body->posr.R, joint->offset );
if ( joint->node[1].body )
{
dVector3 anchor2;
// get the anchor2 in global coordinates
dMultiply0_331( anchor2, joint->node[1].body->posr.R, joint->anchor2 );
q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
( joint->node[1].body->posr.pos[0] + anchor2[0] ) );
q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
( joint->node[1].body->posr.pos[1] + anchor2[1] ) );
q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
( joint->node[1].body->posr.pos[2] + anchor2[2] ) );
}
else
{
//N.B. When there is no body 2 the joint->anchor2 is already in
// global coordinates
q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
( joint->anchor2[0] ) );
q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
( joint->anchor2[1] ) );
q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
( joint->anchor2[2] ) );
if ( joint->flags & dJOINT_REVERSE )
{
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
}
}
dVector3 axP;
// get prismatic axis in global coordinates
dMultiply0_331( axP, joint->node[0].body->posr.R, joint->axisP1 );
return dCalcVectorDot3( axP, q );
}
开发者ID:weilandetian,项目名称:Yoyo,代码行数:51,代码来源:pr.cpp
示例8: getAxis
void getAxis( dxJoint *j, dVector3 result, dVector3 axis1 )
{
if ( j->node[0].body )
{
dMultiply0_331( result, j->node[0].body->posr.R, axis1 );
}
}
开发者ID:nurF,项目名称:Brute-Force-Game-Engine,代码行数:7,代码来源:joint.cpp
示例9: dMassRotate
void dMassRotate (dMass *m, const dMatrix3 R)
{
// if the body is rotated by `R' relative to its point of reference,
// the new inertia about the point of reference is:
//
// R * I * R'
//
// where I is the old inertia.
dMatrix3 t1;
dReal t2[3];
dAASSERT (m);
// rotate inertia matrix
dMultiply2_333 (t1,m->I,R);
dMultiply0_333 (m->I,R,t1);
// ensure perfect symmetry
m->_I(1,0) = m->_I(0,1);
m->_I(2,0) = m->_I(0,2);
m->_I(2,1) = m->_I(1,2);
// rotate center of mass
dMultiply0_331 (t2,R,m->c);
m->c[0] = t2[0];
m->c[1] = t2[1];
m->c[2] = t2[2];
# ifndef dNODEBUG
dMassCheck (m);
# endif
}
开发者ID:Belxjander,项目名称:Asuna,代码行数:33,代码来源:mass.cpp
示例10: dMultiply0_331
void dxTriMesh::computeAABB() {
const dxTriMeshData* d = Data;
dVector3 c;
const dMatrix3& R = final_posr->R;
const dVector3& pos = final_posr->pos;
dMultiply0_331( c, R, d->AABBCenter );
dReal xrange = dFabs(R[0] * Data->AABBExtents[0]) +
dFabs(R[1] * Data->AABBExtents[1]) +
dFabs(R[2] * Data->AABBExtents[2]);
dReal yrange = dFabs(R[4] * Data->AABBExtents[0]) +
dFabs(R[5] * Data->AABBExtents[1]) +
dFabs(R[6] * Data->AABBExtents[2]);
dReal zrange = dFabs(R[8] * Data->AABBExtents[0]) +
dFabs(R[9] * Data->AABBExtents[1]) +
dFabs(R[10] * Data->AABBExtents[2]);
aabb[0] = c[0] + pos[0] - xrange;
aabb[1] = c[0] + pos[0] + xrange;
aabb[2] = c[1] + pos[1] - yrange;
aabb[3] = c[1] + pos[1] + yrange;
aabb[4] = c[2] + pos[2] - zrange;
aabb[5] = c[2] + pos[2] + zrange;
}
开发者ID:devrt,项目名称:gazebo-mirror,代码行数:25,代码来源:collision_trimesh_opcode.cpp
示例11: dGeomSetPosition
void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
CHECK_NOT_LOCKED (g->parent_space);
if (g->offset_posr) {
// move body such that body+offset = position
dVector3 world_offset;
dMultiply0_331(world_offset, g->body->posr.R, g->offset_posr->pos);
dBodySetPosition(g->body,
x - world_offset[0],
y - world_offset[1],
z - world_offset[2]);
}
else if (g->body) {
// this will call dGeomMoved (g), so we don't have to
dBodySetPosition (g->body,x,y,z);
}
else {
g->final_posr->pos[0] = x;
g->final_posr->pos[1] = y;
g->final_posr->pos[2] = z;
dGeomMoved (g);
}
}
开发者ID:JohnCrash,项目名称:ode,代码行数:25,代码来源:collision_kernel.cpp
示例12: dMultiply0_331
void
dxJointUniversal::getAxes( dVector3 ax1, dVector3 ax2 )
{
// This says "ax1 = joint->node[0].body->posr.R * joint->axis1"
dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
if ( node[1].body )
{
dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
}
else
{
ax2[0] = axis2[0];
ax2[1] = axis2[1];
ax2[2] = axis2[2];
}
}
开发者ID:weilandetian,项目名称:Yoyo,代码行数:17,代码来源:universal.cpp
示例13: dJointAddHinge2Torques
void dJointAddHinge2Torques( dJointID j, dReal torque1, dReal torque2 )
{
dxJointHinge2* joint = ( dxJointHinge2* )j;
dVector3 axis1, axis2;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Hinge2 );
if ( joint->node[0].body && joint->node[1].body )
{
dMultiply0_331( axis1, joint->node[0].body->posr.R, joint->axis1 );
dMultiply0_331( axis2, joint->node[1].body->posr.R, joint->axis2 );
axis1[0] = axis1[0] * torque1 + axis2[0] * torque2;
axis1[1] = axis1[1] * torque1 + axis2[1] * torque2;
axis1[2] = axis1[2] * torque1 + axis2[2] * torque2;
dBodyAddTorque( joint->node[0].body, axis1[0], axis1[1], axis1[2] );
dBodyAddTorque( joint->node[1].body, -axis1[0], -axis1[1], -axis1[2] );
}
}
开发者ID:emperorstarfinder,项目名称:opensim-libs,代码行数:18,代码来源:hinge2.cpp
示例14: getAnchor
void getAnchor( dxJoint *j, dVector3 result, dVector3 anchor1 )
{
if ( j->node[0].body )
{
dMultiply0_331( result, j->node[0].body->posr.R, anchor1 );
result[0] += j->node[0].body->posr.pos[0];
result[1] += j->node[0].body->posr.pos[1];
result[2] += j->node[0].body->posr.pos[2];
}
}
开发者ID:nurF,项目名称:Brute-Force-Game-Engine,代码行数:10,代码来源:joint.cpp
示例15: setBall
void setBall( dxJoint *joint, dxJoint::Info2 *info,
dVector3 anchor1, dVector3 anchor2 )
{
// anchor points in global coordinates with respect to body PORs.
dVector3 a1, a2;
int s = info->rowskip;
// set jacobian
info->J1l[0] = 1;
info->J1l[s+1] = 1;
info->J1l[2*s+2] = 1;
dMultiply0_331( a1, joint->node[0].body->posr.R, anchor1 );
dSetCrossMatrixMinus( info->J1a, a1, s );
if ( joint->node[1].body )
{
info->J2l[0] = -1;
info->J2l[s+1] = -1;
info->J2l[2*s+2] = -1;
dMultiply0_331( a2, joint->node[1].body->posr.R, anchor2 );
dSetCrossMatrixPlus( info->J2a, a2, s );
}
// set right hand side
dReal k = info->fps * info->erp;
if ( joint->node[1].body )
{
for ( int j = 0; j < 3; j++ )
{
info->c[j] = k * ( a2[j] + joint->node[1].body->posr.pos[j] -
a1[j] - joint->node[0].body->posr.pos[j] );
}
}
else
{
for ( int j = 0; j < 3; j++ )
{
info->c[j] = k * ( anchor2[j] - a1[j] -
joint->node[0].body->posr.pos[j] );
}
}
}
开发者ID:nurF,项目名称:Brute-Force-Game-Engine,代码行数:42,代码来源:joint.cpp
示例16: setFixedOrientation
void setFixedOrientation( dxJoint *joint, dxJoint::Info2 *info, dQuaternion qrel, int start_row )
{
int s = info->rowskip;
int start_index = start_row * s;
// 3 rows to make body rotations equal
info->J1a[start_index] = 1;
info->J1a[start_index + s + 1] = 1;
info->J1a[start_index + s*2+2] = 1;
if ( joint->node[1].body )
{
info->J2a[start_index] = -1;
info->J2a[start_index + s+1] = -1;
info->J2a[start_index + s*2+2] = -1;
}
// compute the right hand side. the first three elements will result in
// relative angular velocity of the two bodies - this is set to bring them
// back into alignment. the correcting angular velocity is
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * u
// = (erp*fps) * theta * u
// where rotation along unit length axis u by theta brings body 2's frame
// to qrel with respect to body 1's frame. using a small angle approximation
// for sin(), this gives
// angular_velocity = (erp*fps) * 2 * v
// where the quaternion of the relative rotation between the two bodies is
// q = [cos(theta/2) sin(theta/2)*u] = [s v]
// get qerr = relative rotation (rotation error) between two bodies
dQuaternion qerr, e;
if ( joint->node[1].body )
{
dQuaternion qq;
dQMultiply1( qq, joint->node[0].body->q, joint->node[1].body->q );
dQMultiply2( qerr, qq, qrel );
}
else
{
dQMultiply3( qerr, joint->node[0].body->q, qrel );
}
if ( qerr[0] < 0 )
{
qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small
qerr[2] = -qerr[2];
qerr[3] = -qerr[3];
}
dMultiply0_331( e, joint->node[0].body->posr.R, qerr + 1 ); // @@@ bad SIMD padding!
dReal k = info->fps * info->erp;
info->c[start_row] = 2 * k * e[0];
info->c[start_row+1] = 2 * k * e[1];
info->c[start_row+2] = 2 * k * e[2];
}
开发者ID:nurF,项目名称:Brute-Force-Game-Engine,代码行数:54,代码来源:joint.cpp
示例17: drawGeom
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
else if (type == dGeomTransformClass) {
dGeomID g2 = dGeomTransformGetGeom (g);
const dReal *pos2 = dGeomGetPosition (g2);
const dReal *R2 = dGeomGetRotation (g2);
dVector3 actual_pos;
dMatrix3 actual_R;
dMultiply0_331 (actual_pos,R,pos2);
actual_pos[0] += pos[0];
actual_pos[1] += pos[1];
actual_pos[2] += pos[2];
dMultiply0_333 (actual_R,R,R2);
drawGeom (g2,actual_pos,actual_R,0);
}
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
开发者ID:nurF,项目名称:Brute-Force-Game-Engine,代码行数:54,代码来源:demo_moving_trimesh.cpp
示例18: dGeomGetPosition
/*******************************************************************************
Function to draw a geometry object.
*******************************************************************************/
void GOdeObject::drawGeom( dGeomID g, const dReal *position, const dReal *orientation ) const
{
if( !g ) //If the geometry object is missing, end the function.
return;
if( !position ) //Position was not passed?
position = dGeomGetPosition( g ); //Then, get the geometry position.
if( !orientation ) //Orientation was not given?
orientation = dGeomGetRotation( g ); //And get existing geometry orientation.
int type = dGeomGetClass( g ); //Get the type of geometry.
if( type == dBoxClass ) //Is it a box?
{
dReal sides[3];
dGeomBoxGetLengths( g, sides ); //Get length of sides.
renderBox( sides, position, orientation ); //Render the actual box in environment.
}
if( type == dSphereClass ) //Is it a sphere?
{
dReal radius;
radius = dGeomSphereGetRadius( g ); //Get the radius.
renderSphere( radius, position, orientation ); //Render sphere in environment.
}
if( type == dCapsuleClass )
{
dReal radius;
dReal length;
dGeomCapsuleGetParams( g, &radius, &length ); //Get both radius and length.
renderCapsule( radius, length, position, orientation ); //Render capsule in environment.
}
if( type == dGeomTransformClass ) //Is it an embeded geom in a composite body.
{
dGeomID g2 = dGeomTransformGetGeom( g ); //Get the actual geometry inside the wrapper.
const dReal *position2 = dGeomGetPosition( g2 ); //Get position and orientation of wrapped geometry.
const dReal *orientation2 = dGeomGetRotation( g2 );
dVector3 actualPosition; //Real world coordinated position and orientation
dMatrix3 actualOrientation; //of the wrapped geometry.
dMultiply0_331( actualPosition, orientation, position2 ); //Get world coordinates of geometry position.
actualPosition[0] += position[0];
actualPosition[1] += position[1];
actualPosition[2] += position[2];
dMultiply0_333( actualOrientation, orientation, orientation2 ); //Get world coordinates of geom orientation.
drawGeom( g2, actualPosition, actualOrientation ); //Draw embeded geometry.
}
}
开发者ID:rdoolabh,项目名称:biomechanical-salamander,代码行数:57,代码来源:GOdeObject.cpp
示例19: dJointGetHinge2Axis2
void dJointGetHinge2Axis2( dJointID j, dVector3 result )
{
dxJointHinge2* joint = ( dxJointHinge2* )j;
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
checktype( joint, Hinge2 );
if ( joint->node[1].body )
{
dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis2 );
}
}
开发者ID:emperorstarfinder,项目名称:opensim-libs,代码行数:11,代码来源:hinge2.cpp
示例20: dJointGetScrewPosition
dReal dJointGetScrewPosition ( dJointID j )
{
dxJointScrew* joint = ( dxJointScrew* ) j;
dUASSERT ( joint, "bad joint argument" );
checktype ( joint, Screw );
// get axis1 in global coordinates
dVector3 ax1, q;
if (!joint->node[0].body)
return 0;
dMultiply0_331 ( ax1, joint->node[0].body->posr.R, joint->axis1 );
if (joint->node[1].body)
{
// get body2 + offset point in global coordinates
dMultiply0_331 ( q, joint->node[1].body->posr.R, joint->offset );
for (int i = 0; i < 3; ++i )
q[i] = joint->node[0].body->posr.pos[i]
- q[i]
- joint->node[1].body->posr.pos[i];
}
else
{
q[0] = joint->node[0].body->posr.pos[0] - joint->offset[0];
q[1] = joint->node[0].body->posr.pos[1] - joint->offset[1];
q[2] = joint->node[0].body->posr.pos[2] - joint->offset[2];
if ( joint->flags & dJOINT_REVERSE )
{
// N.B. it could have been simplier to only inverse the sign of
// the dCalcVectorDot3 result but this case is exceptional and
// doing the check for all case can decrease the performance.
ax1[0] = -ax1[0];
ax1[1] = -ax1[1];
ax1[2] = -ax1[2];
}
}
return dCalcVectorDot3 ( ax1, q );
}
开发者ID:mylxiaoyi,项目名称:gazebo,代码行数:41,代码来源:screw.cpp
注:本文中的dMultiply0_331函数示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论