当前位置: 首页>>代码示例>>C++>>正文

C++ dCalcVectorDot3函数代码示例

本文整理汇总了C++中dCalcVectorDot3函数的典型用法代码示例。如果您正苦于以下问题：C++ dCalcVectorDot3函数的具体用法？C++ dCalcVectorDot3怎么用？C++ dCalcVectorDot3使用的例子？那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。


在下文中一共展示了dCalcVectorDot3函数的15个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的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


注：本文中的dCalcVectorDot3函数示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台，相关代码片段筛选自各路编程大神贡献的开源项目，源码版权归原作者所有，传播和使用请参考对应项目的License；未经允许，请勿转载。