prapid.cpp

用于GPU通用计算的编程语言BrookGPU 0.4

  s = T.x * B.m12 + T.y * B.m22 + T.z * B.m32;
  t = ABS (s);

  r &= (t <= (b.y + a.x * Bf.m12 + a.y * Bf.m22 + a.z * Bf.m32));
  if (!r) return 5;

  // B0 x B1 = B2
  s = T.x * B.m13 + T.y * B.m23 + T.z * B.m33;
  t = ABS (s);

  r &= (t <= (b.z + a.x * Bf.m13 + a.y * Bf.m23 + a.z * Bf.m33));
  if (!r) return 6;

  // A0 x B0
  s = T.z * B.m21 - T.y * B.m31;
  t = ABS (s);

  r &= (t <= (a.y * Bf.m31 + a.z * Bf.m21 + b.y * Bf.m13 + b.z * Bf.m12));
  if (!r) return 7;

  // A0 x B1
  s = T.z * B.m22 - T.y * B.m32;
  t = ABS (s);

  r &= (t <= (a.y * Bf.m32 + a.z * Bf.m22 + b.x * Bf.m13 + b.z * Bf.m11));
  if (!r) return 8;

  // A0 x B2
  s = T.z * B.m23 - T.y * B.m33;
  t = ABS (s);

  r &= (t <= (a.y * Bf.m33 + a.z * Bf.m23 + b.x * Bf.m12 + b.y * Bf.m11));
  if (!r) return 9;

  // A1 x B0
  s = T.x * B.m31 - T.z * B.m11;
  t = ABS (s);

  r &= (t <= (a.x * Bf.m31 + a.z * Bf.m11 + b.y * Bf.m23 + b.z * Bf.m22));
  if (!r) return 10;

  // A1 x B1
  s = T.x * B.m32 - T.z * B.m12;
  t = ABS (s);

  r &= (t <= (a.x * Bf.m32 + a.z * Bf.m12 + b.x * Bf.m23 + b.z * Bf.m21));
  if (!r) return 11;

  // A1 x B2
  s = T.x * B.m33 - T.z * B.m13;
  t = ABS (s);

  r &= (t <= (a.x * Bf.m33 + a.z * Bf.m13 + b.x * Bf.m22 + b.y * Bf.m21));
  if (!r) return 12;

  // A2 x B0
  s = T.y * B.m11 - T.x * B.m21;
  t = ABS (s);

  r &= (t <= (a.x * Bf.m21 + a.y * Bf.m11 + b.y * Bf.m33 + b.z * Bf.m32));
  if (!r) return 13;

  // A2 x B1
  s = T.y * B.m12 - T.x * B.m22;
  t = ABS (s);

  r &= (t <= (a.x * Bf.m22 + a.y * Bf.m12 + b.x * Bf.m33 + b.z * Bf.m31));
  if (!r) return 14;

  // A2 x B2
  s = T.y * B.m13 - T.x * B.m23;
  t = ABS (s);

  r &= (t <= (a.x * Bf.m23 + a.y * Bf.m13 + b.x * Bf.m32 + b.y * Bf.m31));
  if (!r) return 15;

  return 0;  // should equal 0
}
std::vector<std::vector<csTraverser> > guide;

#if 0

int csRapidCollider::CollideRecursive (csCdBBox *b1, csCdBBox *b2,
	const csMatrix3& R, const csVector3& T)
{
  int rc;      // return codes
  if (0&&csRapidCollider::firstHit && (csRapidCollider::numHits > 0))
    return false;

  // test top level
  csRapidCollider::boxesTested++;

  int f1 = obb_disjoint (R, T, b1->GetRadius(), b2->GetRadius());

  if (f1 != 0)
    return false;  // stop processing this test, go to top of loop

  // contact between boxes
  if (b1->IsLeaf() && b2->IsLeaf())
  {
    // it is a leaf pair - compare the polygons therein
    // TrianglesHaveContact uses the model-to-model transforms stored in
    // csRapidCollider::mR, csRapidCollider::mT.

    // this will pass along any OUT_OF_MEMORY return codes which
    // may be generated.
    return csCdBBox::TrianglesHaveContact(b1, b2);
  }

  csMatrix3 cR;
  csVector3 cT (0, 0, 0);

  // Currently, the transform from model 2 to model 1 space is
  // given by [B T], where y = [B T].x = B.x + T.

  if (b2->IsLeaf() || (!b1->IsLeaf() && (b1->GetSize() > b2->GetSize())))
  {
    // here we descend to children of b1.  The transform from
    // a child of b1 to b1 is stored in
    // [b1->N->m_Rotation,b1->N->m_Translation],
    // but we will denote it [B1 T1] for short.

    // Here, we compute [B1 T1]'[B T] = [B1'B B1'(T-T1)]
    // for each child, and store the transform into the collision
    // test queue.

    csMatrix3 rot_transp = b1->m_pChild0->m_Rotation.GetTranspose ();
    cR = rot_transp * R;
    cT = rot_transp * (T - b1->m_pChild0->m_Translation);

    if ((rc = CollideRecursive (b1->m_pChild0, b2, cR, cT)) != false)
      return rc;

    rot_transp = b1->m_pChild1->m_Rotation.GetTranspose ();
    cR = rot_transp * R;
    cT = rot_transp * (T - b1->m_pChild1->m_Translation);

    if ((rc = CollideRecursive (b1->m_pChild1, b2, cR, cT)) != false)
      return rc;
	
  }
  else
  {
    // here we descend to the children of b2.  See comments for
    // other 'if' clause for explanation.
    cR = R * b2->m_pChild0->m_Rotation;
    cT = ( R * b2->m_pChild0->m_Translation) + T;

    if ((rc = CollideRecursive (b1, b2->m_pChild0, cR, cT)) != false)
      return rc;
 
    cR = R * b2->m_pChild1->m_Rotation;
    cT = ( R * b2->m_pChild1->m_Translation) + T;
	
    if ((rc = CollideRecursive (b1, b2->m_pChild1, cR, cT)) != false)
      return rc;
	
  }

  return false;
}
#else
static void Barken() {

}
static void Bark() {

}
int csRapidCollider::CollideRecursive (csCdBBox *b1, csCdBBox *b2,
                                       const csMatrix3& R, const csVector3& T)
{
  std::queue <csTraverser> breadth;
  
  unsigned int pass_count=0;
  unsigned int pass_size=1;
  unsigned int num_passes=0;
  std::vector<csTraverser> temp_guide;
  breadth.push(csTraverser(b1,b2,R,T));
  while (breadth.size()) {
  if (pass_count==pass_size) {
    pass_size=breadth.size();
    pass_count=0;
    static int lastCount=0;
    static int lastCheckCount=0;
    printf ("Pass %d Num Triangles %d num nodes %d\n",num_passes,csRapidCollider::trianglesTested-lastCheckCount,pass_size);
    if (csRapidCollider::numHits-lastCheckCount) {
         printf ("Detected %d hits\n",csRapidCollider::numHits-lastCount);      
    }
    guide.push_back(temp_guide);
    temp_guide.clear();
    lastCount = csRapidCollider::numHits;
    lastCheckCount = csRapidCollider::trianglesTested;
    num_passes++;
  }
  pass_count++;
  csCdBBox *b1; csCdBBox *b2;
  csMatrix3 R;csVector3 T;
  b1 = breadth.front().b1;
  b2 = breadth.front().b2;
  R = breadth.front().R;
  T = breadth.front().T;
  //if (pass_count!=1)
  temp_guide.push_back(breadth.front());
  breadth.pop();
  // test top level
  csRapidCollider::boxesTested++;
  if (num_passes==12) {
    if (pass_count==216) {
      Barken();
    }
    if (pass_count==186)
      Bark();
  }
  int f1 = obb_disjoint (R, T, b1->GetRadius(), b2->GetRadius());

  if (f1 != 0)
    continue;  // stop processing this test, go to top of loop

  // contact between boxes
  if (b1->IsLeaf() && b2->IsLeaf())
  {
    // it is a leaf pair - compare the polygons therein
    // TrianglesHaveContact uses the model-to-model transforms stored in
    // csRapidCollider::mR, csRapidCollider::mT.

    // this will pass along any OUT_OF_MEMORY return codes which
    // may be generated.
    csCdBBox::TrianglesHaveContact(b1, b2);
    continue;
  }

  csMatrix3 cR;
  csVector3 cT (0, 0, 0);

  // Currently, the transform from model 2 to model 1 space is
  // given by [B T], where y = [B T].x = B.x + T.

  if (b2->IsLeaf() || (!b1->IsLeaf() && (b1->GetSize() > b2->GetSize())))
  {
    // here we descend to children of b1.  The transform from
    // a child of b1 to b1 is stored in
    // [b1->N->m_Rotation,b1->N->m_Translation],
    // but we will denote it [B1 T1] for short.

    // Here, we compute [B1 T1]'[B T] = [B1'B B1'(T-T1)]
    // for each child, and store the transform into the collision
    // test queue.

    csMatrix3 rot_transp = b1->m_pChild0->m_Rotation.GetTranspose ();
    cR = rot_transp * R;
    cT = rot_transp * (T - b1->m_pChild0->m_Translation);

    //if ((rc = CollideRecursive (b1->m_pChild0, b2, cR, cT)) != false)
    //      return rc;
    breadth.push(csTraverser(b1->m_pChild0, b2, cR, cT));

    rot_transp = b1->m_pChild1->m_Rotation.GetTranspose ();
    cR = rot_transp * R;
    cT = rot_transp * (T - b1->m_pChild1->m_Translation);

    breadth.push(csTraverser(b1->m_pChild1, b2, cR, cT));
	
  }
  else
  {
    // here we descend to the children of b2.  See comments for
    // other 'if' clause for explanation.
    cR = R * b2->m_pChild0->m_Rotation;
    cT = ( R * b2->m_pChild0->m_Translation) + T;

    breadth.push(csTraverser (b1, b2->m_pChild0, cR, cT));
 
    cR = R * b2->m_pChild1->m_Rotation;
    cT = ( R * b2->m_pChild1->m_Translation) + T;
	
    breadth.push (csTraverser(b1, b2->m_pChild1, cR, cT));
	
  }
  }
  return false;
}

#endif
typedef struct traverser_t {
  float4 index;//.xy is index into the aTree  .zw is index into bTree
  float4 translation; 
  float4 rotationX;
  float4 rotationY;
  float3 Rotationx() {return float3(rotationX.x,rotationX.y,rotationX.z);}
  float3 Rotationy() {return float3(rotationY.x,rotationY.y,rotationY.z);}
  float3 Rotationz() {return float3(rotationX.w,rotationY.w,translation.w);}
  float3 Translation() {return float3(translation.x,
                                      translation.y,
                                      translation.z);}
}Traverser;
bool assertf(bool b) {
  if (b) {
    return false;
  }return true;
}
void assertv(csVector3 a, float3 b) {
  assert(fabs(a.x-b.x)<.0001);
  assert(fabs(a.y-b.y)<.0001);
  assert(fabs(a.y-b.y)<.0001);
}
bool myeq(float x, float y) {
  return fabs(x-y)<.001;
}
class PassSorter{
public:
  bool operator () (const Traverser &x, const Traverser&y) {
    //return x.index.x<y.index.x;
    return x.index.x<y.index.x||
      (x.index.x==y.index.x&&
       (x.index.y<y.index.y||
        (x.index.y==y.index.y&&
         x.index.z<y.index.z||
         (x.index.z==y.index.z&&
          x.index.w<y.index.w))));
  }
};
class csPassSorter{
public:
  bool operator () (const csTraverser &x, const csTraverser&y) {
    return x.b1[0].ind[0]<y.b1[0].ind[0]||
      (x.b1[0].ind[0]==y.b1[0].ind[0]&&
       (x.b1[0].ind[1]<y.b1[0].ind[1]||
        (x.b1[0].ind[1]==y.b1[0].ind[1]&&
         x.b2[0].ind[0]<y.b2[0].ind[0]||
         (x.b2[0].ind[0]==y.b2[0].ind[0]&&
          x.b2[0].ind[1]<y.b2[0].ind[1]))));
   
  }
};
int checkPassCorrectness(Traverser * traverser,int numTraverser, int numpass) {
  int error_count=0;

  if (numpass==0||numpass==1||numpass>=(int)guide.size())
    return 1;
  std::vector<csTraverser>* b = &guide[numpass];
  bool dosort=true;
  if (dosort) {
    std::sort(traverser,traverser+numTraverser,PassSorter());
    std::sort(b->begin(),b->end(),csPassSorter());
  }
  int j=0;
  for (int i=0;j<(int)b->size()&&i<numTraverser;++i) {
    csTraverser csTrav = (*b)[j];
    Traverser trav=traverser[i];
    if (trav.index.y==0&&trav.index.x<1.5&&trav.index.x>.5) {
      //if (!dosort)++j;
      continue;
    }
    j++;
    if (assertf ( trav.index.x == csTrav.b1->ind[0])
        ||assertf ( trav.index.y == csTrav.b1->ind[1])
        ||assertf ( trav.index.z == csTrav.b2->ind[0])
        ||assertf ( trav.index.w == csTrav.b2->ind[1])) {
      Traverser tmp;
      tmp.index.x = csTrav.b1->ind[0];
      tmp.index.y = csTrav.b1->ind[1];
      tmp.index.z = csTrav.b2->ind[0];
      tmp.index.w = csTrav.b2->ind[1];
      error_count++;
      PassSorter p;
      if (p(tmp,trav)) {
        i--;        
      }else {
        j--;
      }

      continue;
    }
    csVector3 rX = csTrav.R.Row1();
    csVector3 rY = csTrav.R.Row2();
    csVector3 rZ = csTrav.R.Row3();
    csVector3 T = csTrav.T;
    //assertv(rX,trav.Rotationx());
    //assertv(rY,trav.Rotationy());
    //assertv(rZ,trav.Rotationz());
    //assertv(T,trav.Translation());
  }
  if (error_count)
    printf ("error detected %d\n",error_count);
      
  //assert (numTraverser==(int)guide[numpass].size());
  return 1;
}

void csRapidCollider::CollideReset (void)
{
  hits = 0;
  currHit = 0;
}

int csRapidCollider::Report (csRapidCollider **id1, csRapidCollider **id2)
{
  if (currHit >= hits) return 0;

  *id1 = hitv [currHit] [0];
  *id2 = hitv [currHit] [1];
  currHit++;
  return 1;
}

bool csCdBBox::TrianglesHaveContact(csCdBBox *pBox1, csCdBBox *pBox2)
{
  // assume just one triangle in each box.

  // the vertices of the CDTriangle in b2 is in model1 C.S.  The vertices of
  // the other triangle is in model2 CS.
  // Use csRapidCollider::mR, csRapidCollider::mT, and to transform into
  // model2 CS.

  int rc;  // return code

  csVector3 i1 =
    ((csRapidCollider::mR * pBox1->m_pTriangle->p1) + csRapidCollider::mT);
  csVector3 i2 =
    ((csRapidCollider::mR * pBox1->m_pTriangle->p2) + csRapidCollider::mT);
  csVector3 i3 =
    ((csRapidCollider::mR * pBox1->m_pTriangle->p3) + csRapidCollider::mT);

  csRapidCollider::trianglesTested++;

  bool f = ::tri_contact(i1, i2, i3, 
                         pBox2->m_pTriangle->p1, 
                         pBox2->m_pTriangle->p2, 
                         pBox2->m_pTriangle->p3)?true:false;

  if (f)
  {
    // add_collision may be unable to allocate enough memory,
    // so be prepared to pass along an OUT_OF_MEMORY return code.
    if ((rc = add_collision(pBox1->m_pTriangle, pBox2->m_pTriangle)) != false)
      return rc?true:false;
  }

  return false;
}

const csVector3 &csRapidCollider::GetRadius() const 
{
  return GetBbox()->GetRadius();
}

const csCdBBox* csRapidCollider::GetBbox(void) const 
{
  return m_pCollisionModel->GetTopLevelBox();
}