scalar vpSystem::GetPotentialEnergy(void) const
{
	int i;
	scalar PotentialEnergy = SCALAR_0;
	
	for ( i = 0; i < m_pJoint.size(); i++ )
		PotentialEnergy += m_pJoint[i]->GetPotentialEnergy();
	
	for ( i = 0; i < m_pBody.size(); i++ )
		if ( !m_pBody[i]->IsGround() ) PotentialEnergy -= m_pBody[i]->m_sI.GetMass() * Inner(m_pBody[i]->m_sFrame * m_pBody[i]->m_sCenterOfMass, m_pWorld->m_sGravity);
	
	return PotentialEnergy;
}
Пример #2
0
void dLineClosestApproach (const dVector3 pa, const dVector3 ua,
                           const dVector3 pb, const dVector3 ub,
                           double *alpha, double *beta)
{
  dVector3 p;
  p[0] = pb[0] - pa[0];
  p[1] = pb[1] - pa[1];
  p[2] = pb[2] - pa[2];
  double uaub = Inner(ua,ub);
  double q1 =  Inner(ua,p);
  double q2 = -Inner(ub,p);
  double d = 1-uaub*uaub;
  if (d <= 0) {
    // @@@ this needs to be made more robust
    *alpha = 0;
    *beta  = 0;
  }
  else {
    d = dRecip(d);
    *alpha = (q1 + uaub*q2)*d;
    *beta  = (uaub*q1 + q2)*d;
  }
}
Пример #3
0
inline bool is_numerical( boost::optional<Inner> ) { return is_numerical(Inner()); }
Пример #4
0
Inner Outer::MyMethod(Inner arg) {  // expected-error {{unknown type name 'Inner'; did you mean 'Outer::Inner'?}}
  return Inner();
}
Пример #5
0
// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
// generate contact points. this returns 0 if there is no contact otherwise
// it returns the number of contacts generated.
// `normal' returns the contact normal.
// `depth' returns the maximum penetration depth along that normal.
// `return_code' returns a number indicating the type of contact that was
// detected:
//        1,2,3 = box 2 intersects with a face of box 1
//        4,5,6 = box 1 intersects with a face of box 2
//        7..15 = edge-edge contact
// `maxc' is the maximum number of contacts allowed to be generated, i.e.
// the size of the `contact' array.
// `contact' and `skip' are the contact array information provided to the
// collision functions. this function only fills in the position and depth
// fields.
int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1,
            const dVector3 p2, const dMatrix3 R2, const dVector3 side2,
            std::vector<Contact>& result)
{
  const double fudge_factor = 1.05;
  dVector3 p,pp,normalC = {0.0, 0.0, 0.0, 0.0};
  const double *normalR = 0;
  double A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
  int i,j,invert_normal,code;

  // get vector from centers of box 1 to box 2, relative to box 1
  p[0] = p2[0] - p1[0];
  p[1] = p2[1] - p1[1];
  p[2] = p2[2] - p1[2];
  dMULTIPLY1_331 (pp,R1,p);		// get pp = p relative to body 1

  // get side lengths / 2
  A[0] = side1[0];
  A[1] = side1[1];
  A[2] = side1[2];
  B[0] = side2[0];
  B[1] = side2[1];
  B[2] = side2[2];

  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
  R11 = Inner44(R1+0,R2+0); R12 = Inner44(R1+0,R2+1); R13 = Inner44(R1+0,R2+2);
  R21 = Inner44(R1+1,R2+0); R22 = Inner44(R1+1,R2+1); R23 = Inner44(R1+1,R2+2);
  R31 = Inner44(R1+2,R2+0); R32 = Inner44(R1+2,R2+1); R33 = Inner44(R1+2,R2+2);

  Q11 = std::abs(R11); Q12 = std::abs(R12); Q13 = std::abs(R13);
  Q21 = std::abs(R21); Q22 = std::abs(R22); Q23 = std::abs(R23);
  Q31 = std::abs(R31); Q32 = std::abs(R32); Q33 = std::abs(R33);

  // for all 15 possible separating axes:
  //   * see if the axis separates the boxes. if so, return 0.
  //   * find the depth of the penetration along the separating axis (s2)
  //   * if this is the largest depth so far, record it.
  // the normal vector will be set to the separating axis with the smallest
  // depth. note: normalR is set to point to a column of R1 or R2 if that is
  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
  // set to a vector relative to body 1. invert_normal is 1 if the sign of
  // the normal should be flipped.

#define TST(expr1,expr2,norm,cc) \
  s2 = std::abs(expr1) - (expr2); \
  if (s2 > s) { \
  s = s2; \
  normalR = norm; \
  invert_normal = ((expr1) < 0); \
  code = (cc); \
}

  s = -1E12;
  invert_normal = 0;
  code = 0;

  // separating axis = u1,u2,u3
  TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
  TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
  TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);

  // separating axis = v1,v2,v3
  TST (Inner41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
  TST (Inner41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
  TST (Inner41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);

  // note: cross product axes need to be scaled when s is computed.
  // normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
  s2 = std::abs(expr1) - (expr2); \
  l = sqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
  if (l > 0) { \
  s2 /= l; \
  if (s2*fudge_factor > s) { \
  s = s2; \
  normalR = 0; \
  normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
  invert_normal = ((expr1) < 0); \
  code = (cc); \
} \
}

  // separating axis = u1 x (v1,v2,v3)
  TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
  TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
  TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);

  // separating axis = u2 x (v1,v2,v3)
  TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
  TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
  TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);

  // separating axis = u3 x (v1,v2,v3)
  TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
  TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
  TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);

#undef TST

  if (!code) return 0;
  if (s > 0.0) return 0;

  // if we get to this point, the boxes interpenetrate. compute the normal
  // in global coordinates.

  Eigen::Vector3d normal;
  Eigen::Vector3d point_vec;
  double penetration;

  if (normalR) {
    normal << normalR[0],normalR[4],normalR[8];
  }
  else {
    normal << Inner((R1),(normalC)), Inner((R1+4),(normalC)), Inner((R1+8),(normalC));
    //dMULTIPLY0_331 (normal,R1,normalC);
  }
  if (invert_normal) {
    normal *= -1.0;
  }



  // compute contact point(s)

  // single point
  if (code > 6) {
    // an edge from box 1 touches an edge from box 2.
    // find a point pa on the intersecting edge of box 1
    dVector3 pa;
    double sign;
    for (i=0; i<3; i++) pa[i] = p1[i];
    for (j=0; j<3; j++)
    {
#define TEMP_INNER14(a,b) (a[0]*(b)[0] + a[1]*(b)[4] + a[2]*(b)[8])
      sign = (TEMP_INNER14(normal,R1+j) > 0) ? 1.0 : -1.0;

      //sign = (Inner14(normal,R1+j) > 0) ? 1.0 : -1.0;

      for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
    }

    // find a point pb on the intersecting edge of box 2
    dVector3 pb;
    for (i=0; i<3; i++) pb[i] = p2[i];
    for (j=0; j<3; j++) {
      sign = (TEMP_INNER14(normal,R2+j) > 0) ? -1.0 : 1.0;
#undef TEMP_INNER14
      for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
    }

    double alpha,beta;
    dVector3 ua,ub;
    for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
    for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];

    dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
    for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
    for (i=0; i<3; i++) pb[i] += ub[i]*beta;


    {
      point_vec << 0.5*(pa[0]+pb[0]), 0.5*(pa[1]+pb[1]), 0.5*(pa[2]+pb[2]);
      penetration = -s;

      Contact contact;
      contact.point = point_vec;
      contact.normal = normal;
      contact.penetrationDepth = penetration;
      result.push_back(contact);
    }
    return 1;
  }

  // okay, we have a face-something intersection (because the separating
  // axis is perpendicular to a face). define face 'a' to be the reference
  // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
  // the incident face (the closest face of the other box).

  const double *Ra,*Rb,*pa,*pb,*Sa,*Sb;
  if (code <= 3) {
    Ra = R1;
    Rb = R2;
    pa = p1;
    pb = p2;
    Sa = A;
    Sb = B;
  }
  else {
    Ra = R2;
    Rb = R1;
    pa = p2;
    pb = p1;
    Sa = B;
    Sb = A;
  }

  // nr = normal vector of reference face dotted with axes of incident box.
  // anr = absolute values of nr.
  dVector3 normal2,nr,anr;
  if (code <= 3) {
    normal2[0] = normal[0];
    normal2[1] = normal[1];
    normal2[2] = normal[2];
  }
  else {
    normal2[0] = -normal[0];
    normal2[1] = -normal[1];
    normal2[2] = -normal[2];
  }
  dMULTIPLY1_331 (nr,Rb,normal2);
  anr[0] = fabs (nr[0]);
  anr[1] = fabs (nr[1]);
  anr[2] = fabs (nr[2]);

  // find the largest compontent of anr: this corresponds to the normal
  // for the indident face. the other axis numbers of the indicent face
  // are stored in a1,a2.
  int lanr,a1,a2;
  if (anr[1] > anr[0]) {
    if (anr[1] > anr[2]) {
      a1 = 0;
      lanr = 1;
      a2 = 2;
    }
    else {
      a1 = 0;
      a2 = 1;
      lanr = 2;
    }
  }
  else {
    if (anr[0] > anr[2]) {
      lanr = 0;
      a1 = 1;
      a2 = 2;
    }
    else {
      a1 = 0;
      a2 = 1;
      lanr = 2;
    }
  }

  // compute center point of incident face, in reference-face coordinates
  dVector3 center;
  if (nr[lanr] < 0) {
    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
  }
  else {
    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
  }

  // find the normal and non-normal axis numbers of the reference box
  int codeN,code1,code2;
  if (code <= 3) codeN = code-1; else codeN = code-4;
  if (codeN==0) {
    code1 = 1;
    code2 = 2;
  }
  else if (codeN==1) {
    code1 = 0;
    code2 = 2;
  }
  else {
    code1 = 0;
    code2 = 1;
  }

  // find the four corners of the incident face, in reference-face coordinates
  double quad[8];	// 2D coordinate of incident face (x,y pairs)
  double c1,c2,m11,m12,m21,m22;
  c1 = Inner14 (center,Ra+code1);
  c2 = Inner14 (center,Ra+code2);
  // optimize this? - we have already computed this data above, but it is not
  // stored in an easy-to-index format. for now it's quicker just to recompute
  // the four dot products.
  m11 = Inner44 (Ra+code1,Rb+a1);
  m12 = Inner44 (Ra+code1,Rb+a2);
  m21 = Inner44 (Ra+code2,Rb+a1);
  m22 = Inner44 (Ra+code2,Rb+a2);
  {
    double k1 = m11*Sb[a1];
    double k2 = m21*Sb[a1];
    double k3 = m12*Sb[a2];
    double k4 = m22*Sb[a2];
    quad[0] = c1 - k1 - k3;
    quad[1] = c2 - k2 - k4;
    quad[2] = c1 - k1 + k3;
    quad[3] = c2 - k2 + k4;
    quad[4] = c1 + k1 + k3;
    quad[5] = c2 + k2 + k4;
    quad[6] = c1 + k1 - k3;
    quad[7] = c2 + k2 - k4;
  }

  // find the size of the reference face
  double rect[2];
  rect[0] = Sa[code1];
  rect[1] = Sa[code2];

  // intersect the incident and reference faces
  double ret[16];
  int n = intersectRectQuad (rect,quad,ret);
  if (n < 1) return 0;		// this should never happen

  // convert the intersection points into reference-face coordinates,
  // and compute the contact position and depth for each point. only keep
  // those points that have a positive (penetrating) depth. delete points in
  // the 'ret' array as necessary so that 'point' and 'ret' correspond.
  //real point[3*8];		// penetrating contact points
  double point[24];		// penetrating contact points
  double dep[8];			// depths for those points
  double det1 = dRecip(m11*m22 - m12*m21);
  m11 *= det1;
  m12 *= det1;
  m21 *= det1;
  m22 *= det1;
  int cnum = 0;			// number of penetrating contact points found
  for (j=0; j < n; j++) {
    double k1 =  m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
    double k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
    for (i=0; i<3; i++)
    {
      point[cnum*3+i] = center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
    }
    dep[cnum] = Sa[codeN] - Inner(normal2,point+cnum*3);
    if (dep[cnum] >= 0) {
      ret[cnum*2] = ret[j*2];
      ret[cnum*2+1] = ret[j*2+1];
      cnum++;
    }
  }
  if (cnum < 1) return 0;	// this should never happen

  // we can't generate more contacts than we actually have
  int maxc = 4;
  if (maxc > cnum) maxc = cnum;
  //if (maxc < 1) maxc = 1;

  if (cnum <= maxc) {
    // we have less contacts than we need, so we use them all
    for (j=0; j < cnum; j++)
    {
      point_vec << point[j*3+0] + pa[0], point[j*3+1] + pa[1], point[j*3+2] + pa[2];

      Contact contact;
      contact.point = point_vec;
      contact.normal = normal;
      contact.penetrationDepth = dep[j];
      result.push_back(contact);
    }
  }
  else {
    // we have more contacts than are wanted, some of them must be culled.
    // find the deepest point, it is always the first contact.
    int i1 = 0;
    double maxdepth = dep[0];
    for (i=1; i<cnum; i++) {
      if (dep[i] > maxdepth) {
        maxdepth = dep[i];
        i1 = i;
      }
    }

    int iret[8];
    cullPoints (cnum,ret,maxc,i1,iret);

    cnum = maxc;
    for (j=0; j < cnum; j++)
    {
      point_vec << point[iret[j]*3+0] + pa[0], point[iret[j]*3+1] + pa[1], point[iret[j]*3+2] + pa[2];

      Contact contact;
      contact.point = point_vec;
      contact.normal = normal;
      contact.penetrationDepth = dep[iret[j]];
      result.push_back(contact);
    }
  }
  return cnum;
}
Пример #6
0
void add_attributes( boost::optional<Inner>, boost::ptr_vector< BaseAttribute >& t ) {
    add_attributes( Inner(), t );
    t.push_back( new Attribute<bool>("is_optional", true) );
}
Пример #7
0
void Entity::Move(ID3D11Device* device, Vector3_t moveAccel, GameWorld* gameWorld, float frameTime)
{
    // CHANGE MOVE_BITMASK TO V3 inAccel
    float dt = frameTime/1000.0f;
    Vector3_t accelVec = {0, 0, 0};

    // Get the rotation in radians to correct in the movement
    float moveDirCos = 1.0f;
    float strafeDirCos = 0.0f;

    // Set the acceleration vector to just the x & z components first for normalization purposes
    accelVec.x = moveAccel.x;
    accelVec.z = moveAccel.z;

    // Normalize the X & Z movement
    float accelLength = LengthSq(accelVec);
    if(accelLength > 1.0f)
    {
        accelVec *= (1.0f / SquareRoot(accelLength));
    }

    // Twiddle factor for movment speed (8.0 is used to combat the drag for movement)
    float entitySpeed = WALK_SPEED*8.0f;
    accelVec *= entitySpeed;

    // TODO(ebd): ODE here!
    if (m_groundDragEn)
    {
        accelVec.x += -8.0f*m_velocity.x;
        accelVec.z += -8.0f*m_velocity.z;  
    }
    else
    {
        accelVec.x += -5.0f*m_velocity.x;
        accelVec.z += -5.0f*m_velocity.z;
    }

    // Now add in the y acceleration value
    accelVec.y = moveAccel.y*entitySpeed*20.0f;

    // Add gravity
    accelVec.y -= GRAVITY_ACCEL;

    // Now do physics based movement
    
    // Need to add Physics objects to the entity
    // RigidBody object -> holds postion, rotation, velocity, accel, and handles movement for the entity
    // Collider object -> holds information about the bounding box and handles collision functions
    // m_RigidBody->Move(&oldPosition);
    // m_Collider->CollisionTest(&position, &rotation);  // This should return a bool for if there was a collision or not
    
    // Do the movement internally for now
    Vector3_t oldPosition = m_position;
    Vector3_t positionDelta = (0.5f*accelVec*Square(dt) + m_velocity*dt);
    
    m_velocity = accelVec*dt + m_velocity;
    m_position = oldPosition + positionDelta;

    // Do minkowski based collision here
    // First get a list of game map tiles to search through
/*
    uint32 minTileX = FloorFloattoUint32(Minimum(m_position.x, oldPosition.x) - m_aabb.x/2);
    uint32 maxTileX = FloorFloattoUint32(Maximum(m_position.x, oldPosition.x) + m_aabb.x/2);
    uint32 minTileZ = FloorFloattoUint32(Minimum(m_position.z, oldPosition.z) - m_aabb.z/2);
    uint32 maxTileZ = FloorFloattoUint32(Maximum(m_position.z, oldPosition.z) + m_aabb.z/2);
*/
    uint32 minTileX = FloorFloattoUint32(Minimum(m_position.x, oldPosition.x));
    uint32 maxTileX = FloorFloattoUint32(Maximum(m_position.x, oldPosition.x));
    uint32 minTileZ = FloorFloattoUint32(Minimum(m_position.z, oldPosition.z));
    uint32 maxTileZ = FloorFloattoUint32(Maximum(m_position.z, oldPosition.z));

    // Set the time remaining on the search to 1.0
    // We use this to find where the collision was along the movement vector
    float tMin = 1.0f;
    Vector3_t collisionNormal = {0.0f, 0.0f, 0.0f};
    bool floorCollision = false;

    // Test each tile in the search space
    for (uint32 tileX = minTileX; tileX <= maxTileX; tileX++)
    {
        for (uint32 tileZ = minTileZ; tileZ <= maxTileZ; tileZ++)
        {
            // Get the normal for the current tile (is it a wall or floor or neither)
            collisionNormal = gameWorld->GetTileNormal(tileX, tileZ);

            // Get the current tile value, and if there is a valid tile then do collision there
            if (collisionNormal.y == 1.0f)
            {
                if (TestFloorCollision(oldPosition, positionDelta, &tMin))
                {
                    collisionNormal = {0.0f, 1.0f, 0.0f};
                    floorCollision = true;
                }     
            } 
        }
    }

    if (!floorCollision)
        collisionNormal = {0.0f, 0.0f, 0.0f};

    // Now update the entity position based on the output of the collision detection
    // Update the timeRemaining using tMin
    m_position.y = oldPosition.y + tMin*positionDelta.y;
    m_velocity = m_velocity - 1*Inner(m_velocity, collisionNormal)*collisionNormal;

/*
    if (m_position.y < 0)
    {
        m_position.y = 0;
        m_velocity.y = 0;
    }
*/

    // Now updat the model position to be where the entity is
    m_Model->UpdatePosition(device, m_position);
}