本文整理汇总了C++中dCalcVectorDot3函数的典型用法代码示例。如果您正苦于以下问题:C++ dCalcVectorDot3函数的具体用法?C++ dCalcVectorDot3怎么用?C++ dCalcVectorDot3使用的例子?那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了dCalcVectorDot3函数的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: dCollideCapsulePlane
int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dCapsuleClass);
dIASSERT (o2->type == dPlaneClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxCapsule *ccyl = (dxCapsule*) o1;
dxPlane *plane = (dxPlane*) o2;
// collide the deepest capping sphere with the plane
dReal sign = (dCalcVectorDot3_14 (plane->p,o1->final_posr->R+2) > 0) ? REAL(-1.0) : REAL(1.0);
dVector3 p;
p[0] = o1->final_posr->pos[0] + o1->final_posr->R[2] * ccyl->lz * REAL(0.5) * sign;
p[1] = o1->final_posr->pos[1] + o1->final_posr->R[6] * ccyl->lz * REAL(0.5) * sign;
p[2] = o1->final_posr->pos[2] + o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign;
dReal k = dCalcVectorDot3 (p,plane->p);
dReal depth = plane->p[3] - k + ccyl->radius;
if (depth < 0) return 0;
contact->normal[0] = plane->p[0];
contact->normal[1] = plane->p[1];
contact->normal[2] = plane->p[2];
contact->pos[0] = p[0] - plane->p[0] * ccyl->radius;
contact->pos[1] = p[1] - plane->p[1] * ccyl->radius;
contact->pos[2] = p[2] - plane->p[2] * ccyl->radius;
contact->depth = depth;
int ncontacts = 1;
if ((flags & NUMC_MASK) >= 2) {
// collide the other capping sphere with the plane
p[0] = o1->final_posr->pos[0] - o1->final_posr->R[2] * ccyl->lz * REAL(0.5) * sign;
p[1] = o1->final_posr->pos[1] - o1->final_posr->R[6] * ccyl->lz * REAL(0.5) * sign;
p[2] = o1->final_posr->pos[2] - o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign;
k = dCalcVectorDot3 (p,plane->p);
depth = plane->p[3] - k + ccyl->radius;
if (depth >= 0) {
dContactGeom *c2 = CONTACT(contact,skip);
c2->normal[0] = plane->p[0];
c2->normal[1] = plane->p[1];
c2->normal[2] = plane->p[2];
c2->pos[0] = p[0] - plane->p[0] * ccyl->radius;
c2->pos[1] = p[1] - plane->p[1] * ccyl->radius;
c2->pos[2] = p[2] - plane->p[2] * ccyl->radius;
c2->depth = depth;
ncontacts = 2;
}
}
for (int i=0; i < ncontacts; i++) {
dContactGeom *currContact = CONTACT(contact,i*skip);
currContact->g1 = o1;
currContact->g2 = o2;
currContact->side1 = -1;
currContact->side2 = -1;
}
return ncontacts;
}
开发者ID:devrt,项目名称:gazebo-mirror,代码行数:60,代码来源:capsule.cpp
示例2: dCollideRayPlane
int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dRayClass);
dIASSERT (o2->type == dPlaneClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxRay *ray = (dxRay*) o1;
dxPlane *plane = (dxPlane*) o2;
dReal alpha = plane->p[3] - dCalcVectorDot3 (plane->p,ray->final_posr->pos);
// note: if alpha > 0 the starting point is below the plane
dReal nsign = (alpha > 0) ? REAL(-1.0) : REAL(1.0);
dReal k = dCalcVectorDot3_14(plane->p,ray->final_posr->R+2);
if (k==0) return 0; // ray parallel to plane
alpha /= k;
if (alpha < 0 || alpha > ray->length) return 0;
contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
contact->normal[0] = nsign*plane->p[0];
contact->normal[1] = nsign*plane->p[1];
contact->normal[2] = nsign*plane->p[2];
contact->depth = alpha;
contact->g1 = ray;
contact->g2 = plane;
contact->side1 = -1;
contact->side2 = -1;
return 1;
}
开发者ID:EdgarSun,项目名称:opende,代码行数:31,代码来源:ray.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: dJointGetHinge2Angle2Rate
dReal dJointGetHinge2Angle2Rate( dJointID j )
{
dxJointHinge2* joint = ( dxJointHinge2* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Hinge2 );
if ( joint->node[0].body && joint->node[1].body )
{
dVector3 axis;
dMultiply0_331( axis, joint->node[1].body->posr.R, joint->axis2 );
dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
if ( joint->node[1].body )
rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
return rate;
}
else return 0;
}
开发者ID:emperorstarfinder,项目名称:opensim-libs,代码行数:16,代码来源:hinge2.cpp
示例5: dCollideSpherePlane
int dCollideSpherePlane (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dSphereClass);
dIASSERT (o2->type == dPlaneClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxSphere *sphere = (dxSphere*) o1;
dxPlane *plane = (dxPlane*) o2;
contact->g1 = o1;
contact->g2 = o2;
contact->side1 = -1;
contact->side2 = -1;
dReal k = dCalcVectorDot3 (o1->final_posr->pos,plane->p);
dReal depth = plane->p[3] - k + sphere->radius;
if (depth >= 0) {
contact->normal[0] = plane->p[0];
contact->normal[1] = plane->p[1];
contact->normal[2] = plane->p[2];
contact->pos[0] = o1->final_posr->pos[0] - plane->p[0] * sphere->radius;
contact->pos[1] = o1->final_posr->pos[1] - plane->p[1] * sphere->radius;
contact->pos[2] = o1->final_posr->pos[2] - plane->p[2] * sphere->radius;
contact->depth = depth;
return 1;
}
else return 0;
}
开发者ID:JohnCrash,项目名称:ode,代码行数:30,代码来源:sphere.cpp
示例6: dJointGetHingeAngleRate
dReal dJointGetHingeAngleRate( dJointID j )
{
dxJointHinge* joint = ( dxJointHinge* )j;
dAASSERT( joint );
checktype( joint, Hinge );
if ( joint->node[0].body )
{
dVector3 axis;
dMultiply0_331( axis, joint->node[0].body->posr.R, joint->axis1 );
dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
if ( joint->flags & dJOINT_REVERSE ) rate = - rate;
return rate;
}
else return 0;
}
开发者ID:arpg,项目名称:Gazebo,代码行数:16,代码来源:hinge.cpp
示例7: 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
示例8: testPlaneSpace
void testPlaneSpace()
{
HEADER;
dVector3 n,p,q;
int bad = 0;
for (int i=0; i<1000; i++) {
dMakeRandomVector (n,3,1.0);
dNormalize3 (n);
dPlaneSpace (n,p,q);
if (fabs(dCalcVectorDot3(n,p)) > tol) bad = 1;
if (fabs(dCalcVectorDot3(n,q)) > tol) bad = 1;
if (fabs(dCalcVectorDot3(p,q)) > tol) bad = 1;
if (fabs(dCalcVectorDot3(p,p)-1) > tol) bad = 1;
if (fabs(dCalcVectorDot3(q,q)-1) > tol) bad = 1;
}
printf ("\t%s\n", bad ? "FAILED" : "passed");
}
开发者ID:Devilmore,项目名称:GoalBabbling,代码行数:17,代码来源:demo_ode.cpp
示例9: dMultiply0_331
dReal
dxJointHinge2::measureAngle1() const
{
// bring axis 2 into first body's reference frame
dVector3 p, q;
if (node[1].body)
dMultiply0_331( p, node[1].body->posr.R, axis2 );
else
dCopyVector3(p, axis2);
if (node[0].body)
dMultiply1_331( q, node[0].body->posr.R, p );
else
dCopyVector3(q, p);
dReal x = dCalcVectorDot3( v1, q );
dReal y = dCalcVectorDot3( v2, q );
return -dAtan2( y, x );
}
开发者ID:Yanzqing,项目名称:BHumanCodeRelease,代码行数:19,代码来源:hinge2.cpp
示例10: dJointGetPRPositionRate
dReal dJointGetPRPositionRate( dJointID j )
{
dxJointPR* joint = ( dxJointPR* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PR );
// get axis1 in global coordinates
dVector3 ax1;
dMultiply0_331( ax1, joint->node[0].body->posr.R, joint->axisP1 );
if ( joint->node[1].body )
{
dVector3 lv2;
dBodyGetRelPointVel( joint->node[1].body, joint->anchor2[0], joint->anchor2[1], joint->anchor2[2], lv2 );
return dCalcVectorDot3( ax1, joint->node[0].body->lvel ) - dCalcVectorDot3( ax1, lv2 );
}
else
{
dReal rate = dCalcVectorDot3( ax1, joint->node[0].body->lvel );
return ( (joint->flags & dJOINT_REVERSE) ? -rate : rate);
}
}
开发者ID:weilandetian,项目名称:Yoyo,代码行数:21,代码来源:pr.cpp
示例11: testNormalize3
void testNormalize3()
{
HEADER;
int i,j,bad=0;
dVector3 n1,n2;
for (i=0; i<1000; i++) {
dMakeRandomVector (n1,3,1.0);
for (j=0; j<3; j++) n2[j]=n1[j];
dNormalize3 (n2);
if (dFabs(dCalcVectorDot3(n2,n2) - 1.0) > tol) bad |= 1;
if (dFabs(n2[0]/n1[0] - n2[1]/n1[1]) > tol) bad |= 2;
if (dFabs(n2[0]/n1[0] - n2[2]/n1[2]) > tol) bad |= 4;
if (dFabs(n2[1]/n1[1] - n2[2]/n1[2]) > tol) bad |= 8;
if (dFabs(dCalcVectorDot3(n2,n1) - dSqrt(dCalcVectorDot3(n1,n1))) > tol) bad |= 16;
if (bad) {
printf ("\tFAILED (code=%x)\n",bad);
return;
}
}
printf ("\tpassed\n");
}
开发者ID:Devilmore,项目名称:GoalBabbling,代码行数:21,代码来源:demo_ode.cpp
示例12: dJointGetPUAngle2Rate
dReal dJointGetPUAngle2Rate( dJointID j )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
if ( joint->node[0].body )
{
dVector3 axis;
if ( joint->flags & dJOINT_REVERSE )
getAxis( joint, axis, joint->axis1 );
else
getAxis2( joint, axis, joint->axis2 );
dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
return rate;
}
return 0;
}
开发者ID:weilandetian,项目名称:Yoyo,代码行数:21,代码来源:pu.cpp
示例13: makeRandomRotation
void makeRandomRotation (dMatrix3 R)
{
dReal *u1 = R, *u2=R+4, *u3=R+8;
dMakeRandomVector (u1,3,1.0);
dNormalize3 (u1);
dMakeRandomVector (u2,3,1.0);
dReal d = dCalcVectorDot3(u1,u2);
u2[0] -= d*u1[0];
u2[1] -= d*u1[1];
u2[2] -= d*u1[2];
dNormalize3(u2);
dCalcVectorCross3(u3,u1,u2);
}
开发者ID:Devilmore,项目名称:GoalBabbling,代码行数:13,代码来源:demo_ode.cpp
示例14: dJointGetScrewPositionRate
dReal dJointGetScrewPositionRate ( dJointID j )
{
dxJointScrew* joint = ( dxJointScrew* ) j;
dUASSERT ( joint, "bad joint argument" );
checktype ( joint, Screw );
// get axis1 in global coordinates
dVector3 ax1;
dMultiply0_331 ( ax1, joint->node[0].body->posr.R, joint->axis1 );
if ( joint->node[1].body )
{
return dCalcVectorDot3 ( ax1, joint->node[0].body->lvel ) -
dCalcVectorDot3 ( ax1, joint->node[1].body->lvel );
}
else
{
dReal rate = dCalcVectorDot3 ( ax1, joint->node[0].body->lvel );
if ( joint->flags & dJOINT_REVERSE ) rate = - rate;
return rate;
}
}
开发者ID:mylxiaoyi,项目名称:gazebo,代码行数:22,代码来源:screw.cpp
示例15: 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
示例16: contactplat_2
// Generate contact info for movement 1
static void contactplat_2(dContact &contact)
{
/*
For arbitrary contact directions we need to project the moving
geom's velocity against the contact normal and fdir1, fdir2
(obtained with dPlaneSpace()). Assuming moving geom=g2
(so the contact joint is in the moving geom's reference frame):
motion1 = dCalcVectorDot3(fdir1, vel);
motion2 = dCalcVectorDot3(fdir2, vel);
motionN = dCalcVectorDot3(normal, vel);
For geom=g1 just negate motionN and motion2. fdir1 is an arbitrary
vector, so there's no need to negate motion1.
*/
contact.surface.mode |=
dContactMotionN | // velocity along normal
dContactMotion1 | dContactMotion2 | // and along the contact plane
dContactFDir1; // don't forget to set the direction 1
// This is a convenience function: given a vector, it finds other 2 perpendicular vectors
dVector3 motiondir1, motiondir2;
dPlaneSpace(contact.geom.normal, motiondir1, motiondir2);
for (int i=0; i<3; ++i)
contact.fdir1[i] = motiondir1[i];
dReal inv = 1;
if (contact.geom.g1 == platform)
inv = -1;
contact.surface.motion1 = dCalcVectorDot3(mov2_vel, motiondir1);
contact.surface.motion2 = inv * dCalcVectorDot3(mov2_vel, motiondir2);
contact.surface.motionN = inv * dCalcVectorDot3(mov2_vel, contact.geom.normal);
}
开发者ID:EdgarSun,项目名称:opende,代码行数:38,代码来源:demo_motion.cpp
示例17: 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
示例18: getHingeAngleFromRelativeQuat
dReal getHingeAngleFromRelativeQuat( dQuaternion qrel, dVector3 axis )
{
// the angle between the two bodies is extracted from the quaternion that
// represents the relative rotation between them. recall that a quaternion
// q is:
// [s,v] = [ cos(theta/2) , sin(theta/2) * u ]
// where s is a scalar and v is a 3-vector. u is a unit length axis and
// theta is a rotation along that axis. we can get theta/2 by:
// theta/2 = atan2 ( sin(theta/2) , cos(theta/2) )
// but we can't get sin(theta/2) directly, only its absolute value, i.e.:
// |v| = |sin(theta/2)| * |u|
// = |sin(theta/2)|
// using this value will have a strange effect. recall that there are two
// quaternion representations of a given rotation, q and -q. typically as
// a body rotates along the axis it will go through a complete cycle using
// one representation and then the next cycle will use the other
// representation. this corresponds to u pointing in the direction of the
// hinge axis and then in the opposite direction. the result is that theta
// will appear to go "backwards" every other cycle. here is a fix: if u
// points "away" from the direction of the hinge (motor) axis (i.e. more
// than 90 degrees) then use -q instead of q. this represents the same
// rotation, but results in the cos(theta/2) value being sign inverted.
// extract the angle from the quaternion. cost2 = cos(theta/2),
// sint2 = |sin(theta/2)|
dReal cost2 = qrel[0];
dReal sint2 = dSqrt( qrel[1] * qrel[1] + qrel[2] * qrel[2] + qrel[3] * qrel[3] );
dReal theta = ( dCalcVectorDot3( qrel + 1, axis ) >= 0 ) ? // @@@ padding assumptions
( 2 * dAtan2( sint2, cost2 ) ) : // if u points in direction of axis
( 2 * dAtan2( sint2, -cost2 ) ); // if u points in opposite direction
// the angle we get will be between 0..2*pi, but we want to return angles
// between -pi..pi
if ( theta > M_PI ) theta -= ( dReal )( 2 * M_PI );
// the angle we've just extracted has the wrong sign
theta = -theta;
return theta;
}
开发者ID:nurF,项目名称:Brute-Force-Game-Engine,代码行数:40,代码来源:joint.cpp
示例19: _dSafeNormalize4
int _dSafeNormalize4 (dVector4 a)
{
dAASSERT (a);
dReal l = dCalcVectorDot3(a,a)+a[3]*a[3];
if (l > 0) {
l = dRecipSqrt(l);
a[0] *= l;
a[1] *= l;
a[2] *= l;
a[3] *= l;
return 1;
}
else {
a[0] = 1;
a[1] = 0;
a[2] = 0;
a[3] = 0;
return 0;
}
}
开发者ID:devrt,项目名称:gazebo-mirror,代码行数:22,代码来源:odemath.cpp
示例20: dOrthogonalizeR
/*
* This takes what is supposed to be a rotation matrix,
* and make sure it is correct.
* Note: this operates on rows, not columns, because for rotations
* both ways give equivalent results.
*/
void dOrthogonalizeR(dMatrix3 m)
{
dReal n0 = dCalcVectorLengthSquare3(m);
if (n0 != 1)
dSafeNormalize3(m);
// project row[0] on row[1], should be zero
dReal proj = dCalcVectorDot3(m, m+4);
if (proj != 0) {
// Gram-Schmidt step on row[1]
m[4] -= proj * m[0];
m[5] -= proj * m[1];
m[6] -= proj * m[2];
}
dReal n1 = dCalcVectorLengthSquare3(m+4);
if (n1 != 1)
dSafeNormalize3(m+4);
/* just overwrite row[2], this makes sure the matrix is not
a reflection */
dCalcVectorCross3(m+8, m, m+4);
m[3] = m[4+3] = m[8+3] = 0;
}
开发者ID:emperorstarfinder,项目名称:opensim-libs,代码行数:29,代码来源:odemath.cpp
注:本文中的dCalcVectorDot3函数示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论