示例#1
0
bool CollisionModel3DImpl::sphereCollision(float origin[3], float radius)
{
  m_NumTriangles = 0;
  bool collided = false;

  m_ColType=Sphere;
  Vector3D O;
  if (m_Static)
    O=Transform(*(Vector3D*)origin,m_InvTransform);
  else
  {
    Matrix3D inv=m_Transform.Inverse();
    O=Transform(*(Vector3D*)origin,inv);
  }
  std::vector<BoxTreeNode*> checks;
  checks.push_back(&m_Root);
  while (!checks.empty())
  {
    BoxTreeNode* b=checks.back();
    checks.pop_back();
    if (b->intersect(O,radius))
    {
      int sons=b->getSonsNumber();
      if (sons)
        while (sons--) checks.push_back(b->getSon(sons));
      else
      {
        int tri=b->getTrianglesNumber();
        while (tri--)
        {
          BoxedTriangle* bt=b->getTriangle(tri);
          Triangle* t=static_cast<Triangle*>(bt);
          if (t->intersect(O,radius,m_ColPoint))
          {
            m_ColTri1[m_NumTriangles++]=*bt;
            m_iColTri1=getTriangleIndex(bt);
			collided = true;
			if(m_NumTriangles > 63)
				return true;
            //return true;
          }
        }
      }
    }
  }
  return collided;
}
	void TetrahedronSetTopologyContainer::createTrianglesInTetrahedronArray()
	{
		if(!hasTriangles()) 
			createTriangleSetArray();

		if(hasTrianglesInTetrahedron())
			clearTrianglesInTetrahedron();

		m_trianglesInTetrahedron.resize( getNumberOfTetrahedra());
		helper::ReadAccessor< Data< sofa::helper::vector<Tetrahedron> > > m_tetrahedron = d_tetrahedron;

		for(unsigned int i = 0; i < m_tetrahedron.size(); ++i)
		{
			const Tetrahedron &t=m_tetrahedron[i];

			// adding triangles in the triangle list of the ith tetrahedron  i
			for (unsigned int j=0; j<4; ++j)
			{
				const int triangleIndex = getTriangleIndex(t[(j+1)%4], t[(j+2)%4], t[(j+3)%4]);
				m_trianglesInTetrahedron[i][j] = (unsigned int) triangleIndex; 
			}
		}
	}
示例#3
0
bool CollisionModel3DImpl::collision(CollisionModel3D* other, 
                                     int AccuracyDepth, 
                                     int MaxProcessingTime,
                                     float* other_transform)
{
  m_ColType=Models;
  CollisionModel3DImpl* o=static_cast<CollisionModel3DImpl*>(other);
  if (!m_Final) throw Inconsistency();
  if (!o->m_Final) throw Inconsistency();
  Matrix3D t=( other_transform==NULL ? o->m_Transform : *((Matrix3D*)other_transform) );
  if (m_Static) t *= m_InvTransform;
  else          t *= m_Transform.Inverse();
  RotationState rs(t);

  if (AccuracyDepth<0) AccuracyDepth=0xFFFFFF;
  if (MaxProcessingTime==0) MaxProcessingTime=0xFFFFFF;
  
  DWORD EndTime,BeginTime = GetTickCount();
  int num=Max(m_Triangles.size(),o->m_Triangles.size());
  int Allocated=Max(64,(num>>4));
  std::vector<Check> checks(Allocated);
  
  int queue_idx=1;
  Check& c=checks[0];
  c.m_first=&m_Root;
  c.depth=0;
  c.m_second=&o->m_Root;
  while (queue_idx>0)
  {
    if (queue_idx>(Allocated/2)) // enlarge the queue.
    {
      Check c;
      checks.insert(checks.end(),Allocated,c);
      Allocated*=2;
    }
    EndTime=GetTickCount();
    if (EndTime >= (BeginTime+MaxProcessingTime)) throw TimeoutExpired();

    // @@@ add depth check
    //Check c=checks.back();
    Check& c=checks[--queue_idx];
    BoxTreeNode* first=c.m_first;
    BoxTreeNode* second=c.m_second;
    assert(first!=NULL);
    assert(second!=NULL);
    if (first->intersect(*second,rs))
    {
      int tnum1=first->getTrianglesNumber();
      int tnum2=second->getTrianglesNumber();
      if (tnum1>0 && tnum2>0)
      {
        {
          for(int i=0;i<tnum2;i++)
          {
            BoxedTriangle* bt2=second->getTriangle(i);
            Triangle tt(Transform(bt2->v1,rs.t),Transform(bt2->v2,rs.t),Transform(bt2->v3,rs.t));
            for(int j=0;j<tnum1;j++)
            {
              BoxedTriangle* bt1=first->getTriangle(j);
              if (tt.intersect(*bt1)) 
              {
                m_ColTri1=*bt1;
                m_iColTri1=getTriangleIndex(bt1);
                m_ColTri2=tt;
                m_iColTri2=o->getTriangleIndex(bt2);
                return true;
              }
            }
          }
        }
      }
      else
      if (first->getSonsNumber()==0)
      {
        BoxTreeNode* s1=second->getSon(0);
        BoxTreeNode* s2=second->getSon(1);
        assert(s1!=NULL);
        assert(s2!=NULL);
        
        Check& c1=checks[queue_idx++];
        c1.m_first=first;
        c1.m_second=s1;

        Check& c2=checks[queue_idx++];
        c2.m_first=first;
        c2.m_second=s2;
      }
      else
      if (second->getSonsNumber()==0)
      {
        BoxTreeNode* f1=first->getSon(0);
        BoxTreeNode* f2=first->getSon(1);
        assert(f1!=NULL);
        assert(f2!=NULL);
        
        Check& c1=checks[queue_idx++];
        c1.m_first=f1;
        c1.m_second=second;

        Check& c2=checks[queue_idx++];
        c2.m_first=f2;
        c2.m_second=second;
      }
      else
      {
        float v1=first->getVolume();
        float v2=second->getVolume();
        if (v1>v2)
        {
          BoxTreeNode* f1=first->getSon(0);
          BoxTreeNode* f2=first->getSon(1);
          assert(f1!=NULL);
          assert(f2!=NULL);

          Check& c1=checks[queue_idx++];
          c1.m_first=f1;
          c1.m_second=second;

          Check& c2=checks[queue_idx++];
          c2.m_first=f2;
          c2.m_second=second;
        }
        else
        {
          BoxTreeNode* s1=second->getSon(0);
          BoxTreeNode* s2=second->getSon(1);
          assert(s1!=NULL);
          assert(s2!=NULL);

          Check& c1=checks[queue_idx++];
          c1.m_first=first;
          c1.m_second=s1;

          Check& c2=checks[queue_idx++];
          c2.m_first=first;
          c2.m_second=s2;
        }
      }
    }
  }
  return false;
}
示例#4
0
bool CollisionModel3DImpl::rayCollision(float origin[3], 
                                        float direction[3],
                                        bool closest,
                                        float segmin, 
                                        float segmax)
{
  float mintparm=9e9f,tparm;
  Vector3D col_point;
  m_ColType=Ray;
  Vector3D O;
  Vector3D D;
  if (m_Static)
  {
    O=Transform(*(Vector3D*)origin,m_InvTransform);
    D=rotateVector(*(Vector3D*)direction,m_InvTransform);
  }
  else
  {
    Matrix3D inv=m_Transform.Inverse();
    O=Transform(*(Vector3D*)origin,inv);
    D=rotateVector(*(Vector3D*)direction,inv);
  }
  if (segmin!=0.0f) // normalize ray
  {
    O+=segmin*D;
    segmax-=segmin;
    segmin=0.0f;
  }
  if (segmax<segmin) 
  {
    D=-D;
    segmax=-segmax;
  }
  std::vector<BoxTreeNode*> checks;
  checks.push_back(&m_Root);
  while (!checks.empty())
  {
    BoxTreeNode* b=checks.back();
    checks.pop_back();
    if (b->intersect(O,D,segmax))
    {
      int sons=b->getSonsNumber();
      if (sons)
        while (sons--) checks.push_back(b->getSon(sons));
      else
      {
        int tri=b->getTrianglesNumber();
        while (tri--)
        {
          BoxedTriangle* bt=b->getTriangle(tri);
          Triangle* t=static_cast<Triangle*>(bt);
          if (t->intersect(O,D,col_point,tparm,segmax)) 
          {
            if (closest)
            {
              if (tparm<mintparm)
              {
                mintparm=tparm;
                m_ColTri1=*bt;
                m_iColTri1=getTriangleIndex(bt);
                m_ColPoint=col_point;
              }
            }
            else
            {
              m_ColTri1=*bt;
              m_iColTri1=getTriangleIndex(bt);
              m_ColPoint=col_point;
              return true;
            }
          }
        }
      }
    }
  }
  if (closest && mintparm<9e9f) return true;
  return false;
}