本文整理汇总了C++中dNormalize3函数的典型用法代码示例。如果您正苦于以下问题:C++ dNormalize3函数的具体用法?C++ dNormalize3怎么用?C++ dNormalize3使用的例子?那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了dNormalize3函数的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: edgeIntersectsRect
static int edgeIntersectsRect (dVector3 v1, dVector3 v2,
dVector3 p1, dVector3 p2, dVector3 p3)
{
int k;
dVector3 u1,u2,n,tmp;
for (k=0; k<3; k++) u1[k] = p3[k]-p1[k];
for (k=0; k<3; k++) u2[k] = p2[k]-p1[k];
dReal d1 = dSqrt(dCalcVectorDot3(u1,u1));
dReal d2 = dSqrt(dCalcVectorDot3(u2,u2));
dNormalize3 (u1);
dNormalize3 (u2);
if (dFabs(dCalcVectorDot3(u1,u2)) > 1e-6) dDebug (0,"bad u1/u2");
dCalcVectorCross3(n,u1,u2);
for (k=0; k<3; k++) tmp[k] = v2[k]-v1[k];
dReal d = -dCalcVectorDot3(n,p1);
if (dFabs(dCalcVectorDot3(n,p1)+d) > 1e-8) dDebug (0,"bad n wrt p1");
if (dFabs(dCalcVectorDot3(n,p2)+d) > 1e-8) dDebug (0,"bad n wrt p2");
if (dFabs(dCalcVectorDot3(n,p3)+d) > 1e-8) dDebug (0,"bad n wrt p3");
dReal alpha = -(d+dCalcVectorDot3(n,v1))/dCalcVectorDot3(n,tmp);
for (k=0; k<3; k++) tmp[k] = v1[k]+alpha*(v2[k]-v1[k]);
if (dFabs(dCalcVectorDot3(n,tmp)+d) > 1e-6) dDebug (0,"bad tmp");
if (alpha < 0) return 0;
if (alpha > 1) return 0;
for (k=0; k<3; k++) tmp[k] -= p1[k];
dReal a1 = dCalcVectorDot3(u1,tmp);
dReal a2 = dCalcVectorDot3(u2,tmp);
if (a1<0 || a2<0 || a1>d1 || a2>d2) return 0;
return 1;
}
开发者ID:JohnCrash,项目名称:ode,代码行数:29,代码来源:demo_collision.cpp
示例2: 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
示例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: FindIntersectionPlanePlane
//plane plane
bool FindIntersectionPlanePlane(const dReal Plane0[4], const dReal Plane1[4],
dVector3 LinePos,dVector3 LineDir)
{
// If Cross(N0,N1) is zero, then either planes are parallel and separated
// or the same plane. In both cases, 'false' is returned. Otherwise,
// the intersection line is
//
// L(t) = t*Cross(N0,N1) + c0*N0 + c1*N1
//
// for some coefficients c0 and c1 and for t any real number (the line
// parameter). Taking dot products with the normals,
//
// d0 = Dot(N0,L) = c0*Dot(N0,N0) + c1*Dot(N0,N1)
// d1 = Dot(N1,L) = c0*Dot(N0,N1) + c1*Dot(N1,N1)
//
// which are two equations in two unknowns. The solution is
//
// c0 = (Dot(N1,N1)*d0 - Dot(N0,N1)*d1)/det
// c1 = (Dot(N0,N0)*d1 - Dot(N0,N1)*d0)/det
//
// where det = Dot(N0,N0)*Dot(N1,N1)-Dot(N0,N1)^2.
/*
Real fN00 = rkPlane0.Normal().SquaredLength();
Real fN01 = rkPlane0.Normal().Dot(rkPlane1.Normal());
Real fN11 = rkPlane1.Normal().SquaredLength();
Real fDet = fN00*fN11 - fN01*fN01;
if ( Math::FAbs(fDet) < gs_fEpsilon )
return false;
Real fInvDet = 1.0f/fDet;
Real fC0 = (fN11*rkPlane0.Constant() - fN01*rkPlane1.Constant())*fInvDet;
Real fC1 = (fN00*rkPlane1.Constant() - fN01*rkPlane0.Constant())*fInvDet;
rkLine.Direction() = rkPlane0.Normal().Cross(rkPlane1.Normal());
rkLine.Origin() = fC0*rkPlane0.Normal() + fC1*rkPlane1.Normal();
return true;
*/
dReal fN00 = dLENGTHSQUARED(Plane0);
dReal fN01 = dDOT(Plane0,Plane1);
dReal fN11 = dLENGTHSQUARED(Plane1);
dReal fDet = fN00*fN11 - fN01*fN01;
if ( fabs(fDet) < fEPSILON)
return false;
dReal fInvDet = 1.0f/fDet;
dReal fC0 = (fN11*Plane0[3] - fN01*Plane1[3])*fInvDet;
dReal fC1 = (fN00*Plane1[3] - fN01*Plane0[3])*fInvDet;
dCROSS(LineDir,=,Plane0,Plane1);
dNormalize3(LineDir);
dVector3 Temp0,Temp1;
dOPC(Temp0,*,Plane0,fC0);
dOPC(Temp1,*,Plane1,fC1);
dOP(LinePos,+,Temp0,Temp1);
return true;
}
开发者ID:Sean3Don,项目名称:opentribot,代码行数:61,代码来源:dCone.cpp
示例5: 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
示例6: setAxes
void setAxes( dxJoint *j, dReal x, dReal y, dReal z,
dVector3 axis1, dVector3 axis2 )
{
if ( j->node[0].body )
{
dReal q[4];
q[0] = x;
q[1] = y;
q[2] = z;
q[3] = 0;
dNormalize3( q );
if ( axis1 )
{
dMultiply1_331( axis1, j->node[0].body->posr.R, q );
axis1[3] = 0;
}
if ( axis2 )
{
if ( j->node[1].body )
{
dMultiply1_331( axis2, j->node[1].body->posr.R, q );
}
else
{
axis2[0] = x;
axis2[1] = y;
axis2[2] = z;
}
axis2[3] = 0;
}
}
}
开发者ID:nurF,项目名称:Brute-Force-Game-Engine,代码行数:32,代码来源:joint.cpp
示例7: fixFrictionVector
void fixFrictionVector( dBodyID b1, dBodyID b2, dContact& contact )
{
dBodyGetPointVel(b1, contact.geom.pos[0], contact.geom.pos[1], contact.geom.pos[2], contact.fdir1);
dVector3 fdir1_b2;
if (b2)
{
dBodyGetPointVel(b2, contact.geom.pos[0], contact.geom.pos[1], contact.geom.pos[2], fdir1_b2);
contact.fdir1[0] -= fdir1_b2[0];
contact.fdir1[1] -= fdir1_b2[1];
contact.fdir1[2] -= fdir1_b2[2];
}
// at this point, contact[i].fdir1 is the relative tangent velocity of the two bodies.
dCROSS(contact.fdir1, =, contact.fdir1, contact.geom.normal);
// now, contact[i].fdir1 is perpendicular to both the normal and
// the relative tangent velocity.
double length = sqrt(contact.fdir1[0] * contact.fdir1[0]
+ contact.fdir1[1] * contact.fdir1[1]
+ contact.fdir1[2] * contact.fdir1[2]);
if (length > 1e-12)
{
// we only use our calculated direction if it has enough precision
contact.fdir1[0] /= length;
contact.fdir1[1] /= length;
contact.fdir1[2] /= length;
dNormalize3(contact.fdir1);
contact.surface.mode |= dContactFDir1;
}
}
开发者ID:fathat,项目名称:game-src,代码行数:29,代码来源:Solver.cpp
示例8: dJointSetGearboxAxis2
void dJointSetGearboxAxis2( dJointID j, dReal x, dReal y, dReal z )
{
dxJointGearbox* joint = static_cast<dxJointGearbox*>(j);
dUASSERT( joint, "bad joint argument" );
dBodyVectorFromWorld(joint->node[1].body, x, y, z, joint->axis2);
dNormalize3(joint->axis2);
}
开发者ID:arpg,项目名称:Gazebo,代码行数:8,代码来源:gearbox.cpp
示例9: test_sphere_point_depth
int test_sphere_point_depth()
{
int j;
dVector3 p,q;
dMatrix3 R;
dReal r,d;
dSimpleSpace space(0);
dGeomID sphere = dCreateSphere (0,1);
dSpaceAdd (space,sphere);
// ********** make a random sphere of radius r at position p
r = dRandReal()+0.1;
dGeomSphereSetRadius (sphere,r);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (sphere,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (sphere,R);
// ********** test center point has depth r
if (dFabs(dGeomSpherePointDepth (sphere,p[0],p[1],p[2]) - r) > tol) FAILED();
// ********** test point on surface has depth 0
for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
dNormalize3 (q);
for (j=0; j<3; j++) q[j] = q[j]*r + p[j];
if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])) > tol) FAILED();
// ********** test point at random depth
d = (dRandReal()*2-1) * r;
for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
dNormalize3 (q);
for (j=0; j<3; j++) q[j] = q[j]*(r-d) + p[j];
if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])-d) > tol) FAILED();
PASSED();
}
开发者ID:JohnCrash,项目名称:ode,代码行数:42,代码来源:demo_collision.cpp
示例10: 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
示例11: dNormalize3
void
dxJointHinge2::getInfo2( dReal worldFPS, dReal worldERP, const Info2Descr *info )
{
// get information we need to set the hinge row
dReal s, c;
dVector3 q;
const dxJointHinge2 *joint = this;
dVector3 ax1, ax2;
joint->getAxisInfo( ax1, ax2, q, s, c );
dNormalize3( q ); // @@@ quicker: divide q by s ?
// set the three ball-and-socket rows (aligned to the suspension axis ax1)
setBall2( this, worldFPS, worldERP, info, anchor1, anchor2, ax1, susp_erp );
// set the hinge row
int s3 = 3 * info->rowskip;
info->J1a[s3+0] = q[0];
info->J1a[s3+1] = q[1];
info->J1a[s3+2] = q[2];
if ( joint->node[1].body )
{
info->J2a[s3+0] = -q[0];
info->J2a[s3+1] = -q[1];
info->J2a[s3+2] = -q[2];
}
// compute the right hand side for the constrained rotational DOF.
// axis 1 and axis 2 are separated by an angle `theta'. the desired
// separation angle is theta0. sin(theta0) and cos(theta0) are recorded
// in the joint structure. the correcting angular velocity is:
// |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize
// = (erp*fps) * (theta0-theta)
// (theta0-theta) can be computed using the following small-angle-difference
// approximation:
// theta0-theta ~= tan(theta0-theta)
// = sin(theta0-theta)/cos(theta0-theta)
// = (c*s0 - s*c0) / (c*c0 + s*s0)
// = c*s0 - s*c0 assuming c*c0 + s*s0 ~= 1
// where c = cos(theta), s = sin(theta)
// c0 = cos(theta0), s0 = sin(theta0)
dReal k = worldFPS * worldERP;
info->c[3] = k * ( c0 * s - joint->s0 * c );
// if the axis1 hinge is powered, or has joint limits, add in more stuff
int row = 4 + limot1.addLimot( this, worldFPS, info, 4, ax1, 1 );
// if the axis2 hinge is powered, add in more stuff
limot2.addLimot( this, worldFPS, info, row, ax2, 1 );
// set parameter for the suspension
info->cfm[0] = susp_cfm;
}
开发者ID:emperorstarfinder,项目名称:opensim-libs,代码行数:54,代码来源:hinge2.cpp
示例12: dJointSetAMotorAxis
void dJointSetAMotorAxis( dJointID j, int anum, int rel, dReal x, dReal y, dReal z )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dAASSERT( joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2 );
checktype( joint, AMotor );
dUASSERT( !( !joint->node[1].body && ( joint->flags & dJOINT_REVERSE ) && rel == 1 ), "no first body, can't set axis rel=1" );
dUASSERT( !( !joint->node[1].body && !( joint->flags & dJOINT_REVERSE ) && rel == 2 ), "no second body, can't set axis rel=2" );
if ( anum < 0 ) anum = 0;
if ( anum > 2 ) anum = 2;
// adjust rel to match the internal body order
if ( !joint->node[1].body && rel == 2 ) rel = 1;
joint->rel[anum] = rel;
// x,y,z is always in global coordinates regardless of rel, so we may have
// to convert it to be relative to a body
dVector3 r;
r[0] = x;
r[1] = y;
r[2] = z;
r[3] = 0;
if ( rel > 0 )
{
if ( rel == 1 )
{
dMultiply1_331( joint->axis[anum], joint->node[0].body->posr.R, r );
}
else
{
// don't assert; handle the case of attachment to a bodiless geom
if ( joint->node[1].body ) // jds
{
dMultiply1_331( joint->axis[anum], joint->node[1].body->posr.R, r );
}
else
{
joint->axis[anum][0] = r[0];
joint->axis[anum][1] = r[1];
joint->axis[anum][2] = r[2];
joint->axis[anum][3] = r[3];
}
}
}
else
{
joint->axis[anum][0] = r[0];
joint->axis[anum][1] = r[1];
joint->axis[anum][2] = r[2];
}
dNormalize3( joint->axis[anum] );
if ( joint->mode == dAMotorEuler ) joint->setEulerReferenceVectors();
}
开发者ID:JdeRobot,项目名称:ThirdParty,代码行数:53,代码来源:amotor.cpp
示例13: dJointPlanarSetPlaneNormal
void dJointPlanarSetPlaneNormal(dJointID joint, dVector3 planeNormal) {
dUASSERT( joint, "bad joint argument" );
checktype( joint, Plane2D );
dxPlanarJoint* planarJoint = (dxPlanarJoint*) joint;
dUASSERT(dLENGTHSQUARED(planeNormal) > 0.000001, "plane normal cannot have zero length");
dCopyVector3(planarJoint->planeNormal, planeNormal);
dNormalize3(planarJoint->planeNormal);
planarJoint->updatePlane();
}
开发者ID:fferri,项目名称:tvs,代码行数:12,代码来源:PlanarJoint.cpp
示例14: dJointSetTransmissionAxis2
void dJointSetTransmissionAxis2( dJointID j, dReal x, dReal y, dReal z )
{
dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
dUASSERT( joint, "bad joint argument" );
dUASSERT(joint->mode = dTransmissionIntersectingAxes,
"can't set individual axes in current mode" );
if (joint->node[1].body) {
dBodyVectorFromWorld(joint->node[1].body, x, y, z, joint->axes[1]);
dNormalize3(joint->axes[1]);
}
joint->update = 1;
}
开发者ID:JohnCrash,项目名称:ode,代码行数:14,代码来源:transmission.cpp
示例15: 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
示例16: _cldTestEdgeCircleAxis
// check for separation between box edge and cylinder circle edge
int _cldTestEdgeCircleAxis( sCylinderBoxData& cData,
const dVector3 &vCenterPoint,
const dVector3 &vVx0, const dVector3 &vVx1,
int iAxis )
{
// calculate direction of edge
dVector3 vDirEdge;
dVector3Subtract(vVx1,vVx0,vDirEdge);
dNormalize3(vDirEdge);
// starting point of edge
dVector3 vEStart;
dVector3Copy(vVx0,vEStart);;
// calculate angle cosine between cylinder axis and edge
dReal fdot2 = dVector3Dot (vDirEdge,cData.vCylinderAxis);
// if edge is perpendicular to cylinder axis
if(dFabs(fdot2) < 1e-5f)
{
// this can't be separating axis, because edge is parallel to circle plane
return 1;
}
// find point of intersection between edge line and circle plane
dVector3 vTemp1;
dVector3Subtract(vCenterPoint,vEStart,vTemp1);
dReal fdot1 = dVector3Dot(vTemp1,cData.vCylinderAxis);
dVector3 vpnt;
vpnt[0]= vEStart[0] + vDirEdge[0] * (fdot1/fdot2);
vpnt[1]= vEStart[1] + vDirEdge[1] * (fdot1/fdot2);
vpnt[2]= vEStart[2] + vDirEdge[2] * (fdot1/fdot2);
// find tangent vector on circle with same center (vCenterPoint) that
// touches point of intersection (vpnt)
dVector3 vTangent;
dVector3Subtract(vCenterPoint,vpnt,vTemp1);
dVector3Cross(vTemp1,cData.vCylinderAxis,vTangent);
// find vector orthogonal both to tangent and edge direction
dVector3 vAxis;
dVector3Cross(vTangent,vDirEdge,vAxis);
// use that vector as separating axis
return _cldTestAxis( cData, vAxis, iAxis );
}
开发者ID:Ricku34,项目名称:ODE.js,代码行数:46,代码来源:collision_cylinder_box.cpp
示例17: test_plane_point_depth
int test_plane_point_depth()
{
int j;
dVector3 n,p,q,a,b; // n = plane normal
dReal d;
dSimpleSpace space(0);
dGeomID plane = dCreatePlane (0,0,0,1,0);
dSpaceAdd (space,plane);
// ********** make a random plane
for (j=0; j<3; j++) n[j] = dRandReal() - 0.5;
dNormalize3 (n);
d = dRandReal() - 0.5;
dGeomPlaneSetParams (plane,n[0],n[1],n[2],d);
dPlaneSpace (n,p,q);
// ********** test point on plane has depth 0
a[0] = dRandReal() - 0.5;
a[1] = dRandReal() - 0.5;
a[2] = 0;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2])) >= tol) FAILED();
// ********** test arbitrary depth point
a[0] = dRandReal() - 0.5;
a[1] = dRandReal() - 0.5;
a[2] = dRandReal() - 0.5;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) + a[2]) >= tol)
FAILED();
// ********** test depth-1 point
a[0] = dRandReal() - 0.5;
a[1] = dRandReal() - 0.5;
a[2] = -1;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) - 1) >= tol) FAILED();
PASSED();
}
开发者ID:JohnCrash,项目名称:ode,代码行数:45,代码来源:demo_collision.cpp
示例18: _cldTestCircleToEdgeAxis
// intersection test between edge and circle
bool _cldTestCircleToEdgeAxis(sData& cData,
const dVector3 &v0, const dVector3 &v1, const dVector3 &v2,
const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1,
const dVector3 &vVx0, const dVector3 &vVx1, int iAxis)
{
// calculate direction of edge
dVector3 vkl;
dVector3Subtract( vVx1 , vVx0 , vkl);
dNormalize3(vkl);
// starting point of edge
dVector3 vol;
dVector3Copy(vVx0,vol);
// calculate angle cosine between cylinder axis and edge
dReal fdot2 = dVector3Dot(vkl , vCylinderAxis1);
// if edge is perpendicular to cylinder axis
if(dFabs(fdot2)<REAL(1e-5))
{
// this can't be separating axis, because edge is parallel to circle plane
return true;
}
// find point of intersection between edge line and circle plane
dVector3 vTemp;
dVector3Subtract(vCenterPoint,vol,vTemp);
dReal fdot1 = dVector3Dot(vTemp,vCylinderAxis1);
dVector3 vpnt;// = vol + vkl * (fdot1/fdot2);
vpnt[0] = vol[0] + vkl[0] * fdot1/fdot2;
vpnt[1] = vol[1] + vkl[1] * fdot1/fdot2;
vpnt[2] = vol[2] + vkl[2] * fdot1/fdot2;
// find tangent vector on circle with same center (vCenterPoint) that touches point of intersection (vpnt)
dVector3 vTangent;
dVector3Subtract(vCenterPoint,vpnt,vTemp);
dVector3Cross(vTemp,vCylinderAxis1,vTangent);
// find vector orthogonal both to tangent and edge direction
dVector3 vAxis;
dVector3Cross(vTangent,vkl,vAxis);
// use that vector as separating axis
return _cldTestAxis( cData ,v0, v1, v2, vAxis, iAxis );
}
开发者ID:atomixnmc,项目名称:simbicon,代码行数:45,代码来源:collision_cylinder_trimesh.cpp
示例19: dJointSetTransmissionAxis
void dJointSetTransmissionAxis( dJointID j, dReal x, dReal y, dReal z )
{
dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
int i;
dUASSERT( joint, "bad joint argument" );
dUASSERT(joint->mode == dTransmissionParallelAxes ||
joint->mode == dTransmissionChainDrive ,
"axes must be set individualy in current mode" );
for (i = 0 ; i < 2 ; i += 1) {
if (joint->node[i].body) {
dBodyVectorFromWorld(joint->node[i].body, x, y, z, joint->axes[i]);
dNormalize3(joint->axes[i]);
}
}
joint->update = 1;
}
开发者ID:JohnCrash,项目名称:ode,代码行数:19,代码来源:transmission.cpp
示例20: 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];
}
dCROSS( ax[1], = , ax[2], ax[0] );
dNormalize3( ax[1] );
}
开发者ID:XudongJin,项目名称:ODE_For_6DOF_Manipulator,代码行数:21,代码来源:amotor.cpp
注:本文中的dNormalize3函数示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论