C++ dDOT函数代码示例

IC	bool  dcTriListCollider::cylinderCrossesLine(const dReal* p,const dReal* R,dReal hlz,
						 const dReal* v0,const dReal* v1,const dReal* l,dVector3 pos){
	dReal _cos=dDOT14(l,R);

	if(!(dFabs(_cos)<1.f)) return false;
	
	dReal sin2=1.f-_cos*_cos;

	dVector3 vp={v0[0]-p[0],v0[1]-p[1],v0[2]-p[2]};
    dReal c1=dDOT(vp,l);
	dReal c2=dDOT14(vp,R);

	dReal t=(c2*_cos-c1)/sin2;
	dReal q=(c2-c1*_cos)/sin2;

	if(dFabs(q)>hlz) return false;

	dVector3 v01={v1[0]-v0[0],v1[1]-v0[1],v1[2]-v0[2]};
	dReal sidelength2=dDOT(v01,v01);

	if(t*t>sidelength2) return false;
	
	pos[0]=v0[0]+l[0]*t;
	pos[1]=v0[1]+l[1]*t;
	pos[2]=v0[2]+l[2]*t;

	return true;
	
}

/*! \brief Returns Whether or not the segment ab intersects plane p
  \param a origin of the segment
  \param b segment destination
  \param p plane to test for intersection
  \param t returns the time "t" in the segment ray that gives us the intersecting 
  point
  \param q returns the intersection point
  \return true if there is an intersection, otherwise false.
*/
bool IntersectSegmentPlane(dVector3 a, 
			   dVector3 b, 
			   dVector4 p, 
			   dReal &t, 
			   dVector3 q)
{
  // Compute the t value for the directed line ab intersecting the plane
  dVector3 ab;
  ab[0]= b[0] - a[0];
  ab[1]= b[1] - a[1];
  ab[2]= b[2] - a[2];
  
  t = (p[3] - dDOT(p,a)) / dDOT(p,ab);
  
  // If t in [0..1] compute and return intersection point
  if (t >= 0.0 && t <= 1.0) 
    {
      q[0] = a[0] + t * ab[0];
      q[1] = a[1] + t * ab[1];
      q[2] = a[2] + t * ab[2];
      return true;
    }
  // Else no intersection
  return false;
}

void dLineClosestApproach (const btVector3 pa, const btVector3 ua,
		const btVector3 pb, const btVector3 ub,
		btScalar *alpha, btScalar *beta)
{
	btVector3 p;
	p[0] = pb[0] - pa[0];
	p[1] = pb[1] - pa[1];
	p[2] = pb[2] - pa[2];
	btScalar uaub = dDOT(ua,ub);
	btScalar q1 =  dDOT(ua,p);
	btScalar q2 = -dDOT(ub,p);
	btScalar d = 1-uaub*uaub;
	if (d <= 0.0001f)
	{
		// @@@ this needs to be made more robust
		*alpha = 0;
		*beta  = 0;
	}
	else
	{
		d = 1.f/d;
		*alpha = (q1 + uaub*q2)*d;
		*beta  = (uaub*q1 + q2)*d;
	}
}

void CPHCapture::CapturedUpdate()
{
    m_island.Unmerge();
    if(m_character->CPHObject::is_active())
    {
        m_taget_element->Enable();
    }

    if(!m_taget_element->isActive()||dDOT(m_joint_feedback.f2,m_joint_feedback.f2)>m_capture_force*m_capture_force)
    {
        Release();
        return;
    }

    float mag=dSqrt(dDOT(m_joint_feedback.f1,m_joint_feedback.f1));

    //m_back_force=m_back_force*0.999f+ ((mag<m_capture_force/5.f) ? mag : (m_capture_force/5.f))*0.001f;
    //
    if(b_character_feedback&&mag>m_capture_force/2.2f)
    {
        float f=mag/(m_capture_force/15.f);
        m_character->ApplyForce(m_joint_feedback.f1[0]/f,m_joint_feedback.f1[1]/f,m_joint_feedback.f1[2]/f);
    }

    Fvector capture_bone_position;
    //CObject* object=smart_cast<CObject*>(m_character->PhysicsRefObject());
    capture_bone_position.set(m_capture_bone->mTransform.c);
    m_character->PhysicsRefObject()->ObjectXFORM().transform_tiny(capture_bone_position);
    dBodySetPosition(m_body,capture_bone_position.x,capture_bone_position.y,capture_bone_position.z);
}

//body - body case
float E_NLD(dBodyID b1,dBodyID b2,const dReal* norm)// norm - from 2 to 1
{
	dMass m1,m2;
	dBodyGetMass(b1,&m1);dBodyGetMass(b2,&m2);
	const dReal* vel1   =dBodyGetLinearVel(b1);
	const dReal* vel2   =dBodyGetLinearVel(b2);

	dReal vel_pr1=dDOT(vel1,norm);
	dReal vel_pr2=dDOT(vel2,norm);

	if(vel_pr1>vel_pr2) return 0.f; //exit if the bodies are departing

	dVector3 impuls1={vel1[0]*m1.mass,vel1[1]*m1.mass,vel1[2]*m1.mass};
	dVector3 impuls2={vel2[0]*m2.mass,vel2[1]*m2.mass,vel2[2]*m2.mass};

	dVector3 c_mas_impuls={impuls1[0]+impuls2[0],impuls1[1]+impuls2[1],impuls1[2]+impuls2[2]};
	dReal cmass=m1.mass+m2.mass;
	dVector3 c_mass_vel={c_mas_impuls[0]/cmass,c_mas_impuls[1]/cmass,c_mas_impuls[2]/cmass};

	dReal c_mass_vel_prg=dDOT(c_mass_vel,norm);

	dReal kin_energy_start=vel_pr1*vel_pr1*m1.mass/2.f+vel_pr2*vel_pr2*m2.mass/2.f;
	dReal kin_energy_end=c_mass_vel_prg*c_mass_vel_prg*cmass/2.f;

	return (kin_energy_start-kin_energy_end);
}

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 = (dDOT14 (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 = dDOT (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 = dDOT (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;
}

// find two closest points on two lines
static BOOL _cldClosestPointOnTwoLines( dVector3 vPoint1, dVector3 vLenVec1, 
                                        dVector3 vPoint2, dVector3 vLenVec2, 
                                        dReal &fvalue1, dReal &fvalue2) 
{
  // calulate denominator
  dVector3 vp;
  SUBTRACT(vPoint2,vPoint1,vp);
  dReal fuaub  = dDOT(vLenVec1,vLenVec2);
  dReal fq1    = dDOT(vLenVec1,vp);
  dReal fq2    = -dDOT(vLenVec2,vp);
  dReal fd     = 1.0f - fuaub * fuaub;
  
  // if denominator is positive
  if (fd > 0.0f) {
    // calculate points of closest approach
    fd = 1.0f/fd;
    fvalue1 = (fq1 + fuaub*fq2)*fd;
    fvalue2 = (fuaub*fq1 + fq2)*fd;
    return TRUE;
  // otherwise  
  } else {
    // lines are parallel
    fvalue1 = 0.0f;
    fvalue2 = 0.0f;
    return FALSE;
  }

}

void BodyCutForce(dBodyID body,float l_limit,float w_limit)
{
	const dReal wa_limit=w_limit/fixed_step;
	const dReal* force=	dBodyGetForce(body);
	dReal force_mag=dSqrt(dDOT(force,force));

	//body mass
	dMass m;
	dBodyGetMass(body,&m);

	dReal force_limit =l_limit/fixed_step*m.mass;

	if(force_mag>force_limit)
	{
		dBodySetForce(body,
			force[0]/force_mag*force_limit,
			force[1]/force_mag*force_limit,
			force[2]/force_mag*force_limit
			);
	}


	const dReal* torque=dBodyGetTorque(body);
	dReal torque_mag=dSqrt(dDOT(torque,torque));

	if(torque_mag<0.001f) return;

	dMatrix3 tmp,invI,I;

	// compute inertia tensor in global frame
	dMULTIPLY2_333 (tmp,m.I,body->R);
	dMULTIPLY0_333 (I,body->R,tmp);

	// compute inverse inertia tensor in global frame
	dMULTIPLY2_333 (tmp,body->invI,body->R);
	dMULTIPLY0_333 (invI,body->R,tmp);

	//angular accel
	dVector3 wa;
	dMULTIPLY0_331(wa,invI,torque);
	dReal wa_mag=dSqrt(dDOT(wa,wa));

	if(wa_mag>wa_limit)
	{
		//scale w 
		for(int i=0;i<3;++i)wa[i]*=wa_limit/wa_mag;
		dVector3 new_torqu;

		dMULTIPLY0_331(new_torqu,I,wa);

		dBodySetTorque 
			(
			body,
			new_torqu[0],
			new_torqu[1],
			new_torqu[2]
			);
	}
}

int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags,
			    dContactGeom *contact, int skip)
{
  dxCapsule *ccyl = (dxCapsule*) o1;
  dxPlane *plane = (dxPlane*) o2;

  // collide the deepest capping sphere with the plane
  dReal sign = (dDOT14 (plane->p,o1->final_posr->R+2) > 0) ? REAL(-1.0) : REAL(1.0);
  dVector3 p;
  p[0] = o1->final_posr->pos[0] + dMUL(o1->final_posr->R[2],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));
  p[1] = o1->final_posr->pos[1] + dMUL(o1->final_posr->R[6],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));
  p[2] = o1->final_posr->pos[2] + dMUL(o1->final_posr->R[10],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));

  dReal k = dDOT (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] - dMUL(plane->p[0],ccyl->radius);
  contact->pos[1] = p[1] - dMUL(plane->p[1],ccyl->radius);
  contact->pos[2] = p[2] - dMUL(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] - dMUL(o1->final_posr->R[2],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));
    p[1] = o1->final_posr->pos[1] - dMUL(o1->final_posr->R[6],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));
    p[2] = o1->final_posr->pos[2] - dMUL(o1->final_posr->R[10],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));

    k = dDOT (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] - dMUL(plane->p[0],ccyl->radius);
      c2->pos[1] = p[1] - dMUL(plane->p[1],ccyl->radius);
      c2->pos[2] = p[2] - dMUL(plane->p[2],ccyl->radius);
      c2->depth = depth;
      ncontacts = 2;
    }
  }

  for (int i=0; i < ncontacts; i++) {
    CONTACT(contact,i*skip)->g1 = o1;
    CONTACT(contact,i*skip)->g2 = o2;
  }
  return ncontacts;
}

//O1,O2 seems to be sphere line intersections
bool dcTriListCollider::circleLineIntersection(const dReal* cn,const dReal* cp,dReal r,const dReal* lv,const dReal* lp,dReal sign,dVector3 point){

	dVector3 LC={lp[0]-cp[0],lp[1]-cp[1],lp[2]-cp[2]};

	dReal A,B,C,B_A,B_A_2,D;
	dReal t1,t2;
	A=dDOT(lv,lv);
	B=dDOT(LC,lv);
	C=dDOT(LC,LC)-r*r;
	B_A=B/A;
	B_A_2=B_A*B_A;
	D=B_A_2-C;
	if(D<0.f){
		
		point[0]=lp[0]-lv[0]*B;
		point[1]=lp[1]-lv[1]*B;
		point[2]=lp[2]-lv[2]*B;
		return false;
		
	}
	else{
	t1=-B_A-_sqrt(D);
	t2=-B_A+_sqrt(D);

	dVector3 O1={lp[0]+lv[0]*t1,lp[1]+lv[1]*t1,lp[2]+lv[2]*t1};
	dVector3 O2={lp[0]+lv[0]*t2,lp[1]+lv[1]*t2,lp[2]+lv[2]*t2};
	//dVector3 test1={O1[0]-cp[0],O1[1]-cp[1],O1[2]-cp[2]};
	//dVector3 test2={O2[0]-cp[0],O2[1]-cp[1],O2[2]-cp[2]};
	//dReal t=_sqrt(dDOT(test1,test1));
	//t=_sqrt(dDOT(test2,test2));

	dReal cpPr=sign*dDOT(cn,cp);

	dReal dist1=(sign*dDOT41(cn,O1)-cpPr);
	dReal dist2=(sign*dDOT41(cn,O2)-cpPr);

	if(dist1<dist2){
					point[0]=O1[0];
					point[1]=O1[1];
					point[2]=O1[2];
					}
	else{			point[0]=O2[0];
					point[1]=O2[1];
					point[2]=O2[2];
					}
					

	return true;
	}
}

dReal dJointGetAMotorAngleRate( dJointID j, int anum )
{
    dxJointAMotor* joint = ( dxJointAMotor* )j;
    dAASSERT( joint && anum >= 0 && anum < 3);
    checktype( joint, AMotor );
  
    if (joint->node[0].body) {
      dVector3 axis;
      dJointGetAMotorAxis (joint, anum, axis);
      dReal rate = dDOT(axis,joint->node[0].body->avel);
      if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel);
      return rate;
    }
    return 0;
}

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] - dDOT (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 = dDOT14(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;
}

//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;
}

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;
  dReal k = dDOT (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;
}

////Energy of non Elastic collision;
//body - static case
float E_NlS(dBodyID body,const dReal* norm,float norm_sign)//if body c.geom.g1 norm_sign + else -
{													 //norm*norm_sign - to body
	const dReal* vel=dBodyGetLinearVel(body);
	dReal prg=-dDOT(vel,norm)*norm_sign;
	prg=prg<0.f ? prg=0.f : prg;
	dMass mass;
	dBodyGetMass(body,&mass);
	return mass.mass*prg*prg/2;
}