prapid.cpp

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

/*
    Copyright (C) 1998,2000 by Written by Alex Pfaffe

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Library General Public
    License as published by the Free Software Foundation; either
    version 2 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Library General Public License for more details.

    You should have received a copy of the GNU Library General Public
    License along with this library; if not, write to the Free
    Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

// The following classes are utility classes for the RAPID collision detection
// algorithm. The code is based on the UNC implementation of the RAPID
// algorithm:

/*************************************************************************\

  Copyright 1995 The University of North Carolina at Chapel Hill.
  All Rights Reserved.

  Permission to use, copy, modify and distribute this software and its
  documentation for educational, research and non-profit purposes, without
  fee, and without a written agreement is hereby granted, provided that the
  above copyright notice and the following three paragraphs appear in all
  copies.

  IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
  LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
  CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
  USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
  OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
  DAMAGES.

  THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE
  PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
  NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
  UPDATES, ENHANCEMENTS, OR MODIFICATIONS.

  The authors may be contacted via:

  US Mail:             S. Gottschalk
                       Department of Computer Science
                       Sitterson Hall, CB #3175
                       University of N. Carolina
                       Chapel Hill, NC 27599-3175

  Phone:               [phone]

  EMail:              {gottscha}@cs.unc.edu


\**************************************************************************/
#include <stdio.h>
#include <queue>
#include <algorithm>
#include "cs_compat.h"
#include "qsqrt.h"
#include "qint.h"
#include "rapcol.h"
#include "prapid.h"
#include "garray.h"

#include "csgeom/transfrm.h"
#define CD_MAX_COLLISION    1000

// This array contains the colliding pairs
static CS_DECLARE_GROWING_ARRAY_REF (CD_contact,csCollisionPair);

static int hits = 0;
// Array of hits.
static csRapidCollider* hitv[CD_MAX_COLLISION][2];
static int currHit;

///
csMatrix3 csRapidCollider::mR;
csVector3 csRapidCollider::mT (0, 0, 0);
///
int   csRapidCollider::trianglesTested = 0;
int   csRapidCollider::boxesTested     = 0;
int   csRapidCollider::testLevel       = 0;
bool  csRapidCollider::firstHit        = true;
int   csRapidCollider::numHits         = 0;
int   csRapidCollider::numTriChecks    = 0;
float csRapidCollider::minBBoxDiam     = 0.0;
///

SCF_IMPLEMENT_IBASE (csRapidCollider)
  SCF_IMPLEMENTS_INTERFACE (iCollider)
SCF_IMPLEMENT_IBASE_END


Moment *Moment::stack = NULL;

csRapidCollider::csRapidCollider (const std::vector <bsp_polygon> &polygons)
{

  SCF_CONSTRUCT_IBASE (NULL);
  GeometryInitialize (polygons);
}

static float min3 (float a, float b, float c)
{ return (a < b ? (a < c ? a : (c < b ? c : b)) : (b < c ? b : c)); }
static float max3(float a, float b, float c)
{ return (a > b ? (a > c ? a : (c > b ? c : b)) : (b > c ? b : c)); }
float3 tofloat3(const csVector3 &a) {
  return float3(a.x,a.y,a.z);
}
void csRapidCollider::createBrookGeometryRecurse(const csCdBBox *curr,
                                                 BBox &curw,
                                                 std::vector<BBox>&bbox,
                                                 std::vector<Tri>&tri){
  float childx,childy;
  curw.Children.x = (float)(bbox.size()%2048);
  curw.Children.y = (float)(bbox.size()/2048);
  childx=curw.Children.x;
  childy=curw.Children.y;
  float3 tmp = tofloat3(curr->m_Rotation.Row1());
  curw.Rotationx.x = tmp.x;
  curw.Rotationx.y = tmp.y;
  curw.Rotationx.z = tmp.z;
  curw.Rotationx.w =((curr->m_Rotation.Row1() % curr->m_Rotation.Row2())*curr->m_Rotation.Row3()>.8)?1.0f:-1.0f;
  curw.Rotationy = tofloat3(curr->m_Rotation.Row2());
  //  curw.Rotationz = tofloat3(curr->m_Rotation.Row3());
  curw.Translation = tofloat3(curr->m_Translation);


  curw.Radius.x = curr->m_Radius.x;
  curw.Radius.y = curr->m_Radius.y;
  curw.Radius.z = curr->m_Radius.z;

  if (curr->IsLeaf()) {
    curw.Radius.w = 1.0f;
    curw.Children.x = (float)(tri.size()%2048);
    curw.Children.y = (float)(tri.size()/2048);
    tri.push_back(Tri());
    tri.back().A = tofloat3(curr->m_pTriangle->p1);
    tri.back().B = tofloat3(curr->m_pTriangle->p2);
    tri.back().C = tofloat3(curr->m_pTriangle->p3);
                            
    //
  }else {
    assert (curr->m_pTriangle==0);
    curw.Radius.w = 0;
    bbox.push_back(BBox());
    unsigned int secondchild = bbox.size();
    bbox.push_back(BBox());
    curr->m_pChild0->ind[0]=childx;
    curr->m_pChild0->ind[1]=childy;
    createBrookGeometryRecurse(curr->m_pChild0,
                               *(bbox.end()-2),
                               bbox,
                               tri);
    curr->m_pChild1->ind[0]=childx+1.0f;
    curr->m_pChild1->ind[1]=childy;
    createBrookGeometryRecurse(curr->m_pChild1,
                               bbox[secondchild],
                               bbox,
                               tri);
  }
}
void csRapidCollider::createBrookGeometry(std::vector<BBox>&bbox,
                                          std::vector<Tri>&tri){
  bbox.push_back(BBox());
  bbox.push_back(BBox());//keep things two-box aligned
  bbox.back().Rotationx=float4(1,0,0,1);
  bbox.back().Rotationy=float3(0,1,0);
  bbox.back().Translation=float3(1024.*1024.*16384,-1024.*1024.*16384,1024.*1024.*16384);
  bbox.back().Radius=float4(1.0f/65536.0f,1.0f/65536.0f,1.0f/65536.0f,1);
  bbox.back().Children=float2(0,0);
  tri.push_back(Tri());
  tri.back().A=float3(1024.*1024.*16384,-1024.*1024.*16384,1024.*1024.*16384);;
  tri.back().B=float3(1025.*1024.*16384,-1024.*1024.*16384,1024.*1024.*16384);
  tri.back().C=float3(1025.*1024.*16384,-1025.*1024.*16384,1024.*1024.*16384);
  createBrookGeometryRecurse(m_pCollisionModel->GetTopLevelBox(),
                             bbox.front(),
                             bbox,
                             tri);
  while (tri.size()%2048)
    tri.push_back(Tri());
  while (bbox.size()%2048)
    bbox.push_back(BBox());
}
void csRapidCollider::GeometryInitialize (const std::vector <bsp_polygon> &polygons)
{
  m_pCollisionModel = NULL;

  CD_contact.IncRef ();

  int i;
  int tri_count = 0;
  // first, count the number of triangles polyset contains
  //  csVector3* vertices = mesh->GetVertices ();
  /*
  for (i = 0; i < (int) polygons.size() ; i++)
  {
    tri_count += polygons[i].v.size() - 2;
  }
  */
  tri_count+=polygons.size();

  csBox3 object_bbox;
  object_bbox.StartBoundingBox ();
  if (tri_count)
  {
    m_pCollisionModel = new csCdModel (tri_count);
    if (!m_pCollisionModel)
      return;
    
    for (i = 0; i < (int) polygons.size() ; i++)
    {
      const bsp_polygon * p = (&polygons[i]);
      //      int* vidx = p.vertices;
      // Collision detection only works with triangles.
      object_bbox.AddBoundingVertex (p->A);
      object_bbox.AddBoundingVertex (p->B);
      //for (v = 2; v < (int)p->v.size(); v++)
      {
        m_pCollisionModel->AddTriangle (p->A,
                                        p->B, 
                                        p->C);
        object_bbox.AddBoundingVertex (p->C);
      }
    }
    m_pCollisionModel->BuildHierarchy ();
  }

  float dx = object_bbox.MaxX () - object_bbox.MinX ();
  float dy = object_bbox.MaxY () - object_bbox.MinY ();
  float dz = object_bbox.MaxZ () - object_bbox.MinZ ();
  smallest_box_dim = min3 (dx, dy, dz);
  if (smallest_box_dim < .1f) smallest_box_dim = .1f;
}

csRapidCollider::~csRapidCollider ()
{

  if (m_pCollisionModel)
  {
    delete m_pCollisionModel;
    m_pCollisionModel = NULL;
  }

  CD_contact.DecRef ();
}

bool csRapidCollider::Collide (csRapidCollider &otherCollider, 
                               const csReversibleTransform *pTransform1, 
                               const csReversibleTransform *pTransform2)
{
  csRapidCollider *pRAPIDCollider2 = (csRapidCollider *)&otherCollider;
// VC does not understand that preprocessor command
//#warning is comparing against self collide tree b4d??
#if 0
  if (pRAPIDCollider2 == this) return false;
#endif
  // JTY: also skip objects with m_pCollisionModel NULL. This fixes
  // a bug with bezier curves and collision detection.
  if (!m_pCollisionModel || 
      !pRAPIDCollider2->m_pCollisionModel) return 0;

  // I don't know, why this is commented out. I need to elaborate this
  // further. thieber 2000-02-19
  //  csRapidCollider::firstHit = true; 

  // call the low level collision detection routine.

  csCdBBox *b1 = m_pCollisionModel->m_pBoxes;
  csCdBBox *b2 = pRAPIDCollider2->m_pCollisionModel->m_pBoxes;

  csMatrix3 R1;
  csVector3 T1 (0, 0, 0);
  if (pTransform1 && pTransform2)
  {
    csReversibleTransform trans1 = *pTransform1;
    trans1 *= pTransform2->GetInverse ();
    T1 = trans1.GetO2TTranslation ();
    R1 = trans1.GetO2T ();
  }
  else if (pTransform1)
  {
    T1 = pTransform1->GetO2TTranslation ();
    R1 = pTransform1->GetO2T ();
  }
  else if (pTransform2)
  {
    csReversibleTransform trans1 = pTransform2->GetInverse ();
    T1 = trans1.GetO2TTranslation ();
    R1 = trans1.GetO2T ();
  }

  // [R1,T1] is how the first triangle set is positioned relative to the
  // second one (combination of two transforms) (in world space).
  // [tR1,tT1] and [tR2,tT2] are how the top level boxes are
  // positioned in world space

  csMatrix3 tR1 = R1 * b1->m_Rotation;
  csVector3 tT1 = (R1 * b1->m_Translation) + T1;
  csMatrix3 tR2 = b2->m_Rotation;
  csVector3 tT2 = b2->m_Translation;

  // [R,T] is the placement of b2's top level box within
  // the coordinate system of b1's top level box.

  csMatrix3 R = tR1.GetTranspose () * tR2;
  csVector3 T = tR1.GetTranspose () * (tT2 - tT1);

  // To transform tri's from model1's CS to model2's CS use this:
  //    x2 = mR . x1 + mT

  csRapidCollider::mR = R1;
  csRapidCollider::mT = T1;

  // reset the report fields
  csRapidCollider::numHits = 0;
  csRapidCollider::trianglesTested = 0;
  csRapidCollider::boxesTested = 0;
  // make the call
  if (CollideRecursive (b1, b2, R, T))
  {
    // Error.
  }
  else if (csRapidCollider::numHits != 0)
  {
    hitv [hits][0] = this;
    hitv [hits][1] = pRAPIDCollider2;
    hits++;
    return 1;
  }
  return 0;
}

bool csRapidCollider::CollideArray (
  	const csReversibleTransform* trans,
  	int num_colliders,
	iCollider** colliders,
	csReversibleTransform **transforms)
{
  int i;
  for (i = 0 ; i < num_colliders ; i++)
  {
    bool rc = Collide (*(csRapidCollider*)colliders[i],
    	trans, transforms[i]);
    if (rc) return rc;
  }
  return false;
}

int csRapidCollider::CollidePath (
  	const csReversibleTransform* trans,
	csVector3& newpos,
	int num_colliders,
	iCollider** colliders,
	csReversibleTransform** transforms)
{
  // At initialization time (GeometryInitialize()) we calculated
  // the smallest dimension of the object space bounding box (either
  // x, y, or z space). We are going to use half this size to slowly
  // move the object along its path. By using half the size we can
  // be fairly sure that we will not miss anything.

  csReversibleTransform test = *trans;
  csVector3 start = test.GetOrigin ();
  csVector3 end = newpos;
  csVector3 testpos;
  float step = 1.0f / (smallest_box_dim/2.0f);
  float curdist = 0;
  bool rc = false;
  bool firsthit = true;
  for (;;)
  {
    testpos = start+curdist * (end-start);
    test.SetOrigin (testpos);
    CollideReset ();
    numHits = 0;
    rc = CollideArray (&test, num_colliders, colliders, transforms);
    if (rc) break;
    firsthit = false;

    if (curdist >= 1) break;
    curdist += step;
    if (curdist > 1) curdist = 1;
  }

  if (rc)
  {
    // We had a collision.
    if (firsthit)
    {
      // The collision happened on the start point. In that case
      // we cannot move at all. Return -1.
      return -1;
    }

    // Here we try to find more exactly where the collision occured by
    // doing a binary search.
    end = testpos;
    while (csSquaredDist::PointPoint (start, end) > .05)
    {
      testpos = (start+end) / 2;
      test.SetOrigin (testpos);
      CollideReset ();
      numHits = 0;
      rc = CollideArray (&test, num_colliders, colliders, transforms);
      if (rc)
      {
        // Use left segment.
        end = testpos;
      }
      else
      {
        // Use right segment.
	start = testpos;
      }
    }
    // We know that the object can move to the 'start' position safely
    // because of the way we handle the binary search and the starting
    // condition that firsthit == false.
    newpos = start;

    // But first we set the collision detection array to the position
    // which resulted in collision.
    test.SetOrigin (end);
    CollideReset ();
    numHits = 0;
    CollideArray (&test, num_colliders, colliders, transforms);

    return 0;
  }
  else
  {
    // There was no collision.
    return 1;
  }
}

csCollisionPair *csRapidCollider::GetCollisions ()
{
  return CD_contact.GetArray ();
}

int project6 (csVector3 ax, csVector3 p1, csVector3 p2, csVector3 p3,
  csVector3 q1, csVector3 q2, csVector3 q3)
{
  float P1 = ax * p1;
  float P2 = ax * p2;