void Leaf::fitBox(const mat3 &R, vec3 &center, vec3 &halfSize)
{
	vec3 x = R.row(0);
	vec3 y = R.row(1);
	vec3 z = R.row(2);
	vec3 max(-1.0e10, -1.0e10, -1.0e10);
	vec3 min( 1.0e10,  1.0e10,  1.0e10);
	std::list<Triangle>::iterator it;
	for (it=mTriangles.begin(); it!=mTriangles.end(); it++) {
		boxSize( (*it).v1, min, max, x, y, z, TOLERANCE);
		boxSize( (*it).v2, min, max, x, y, z, TOLERANCE);
		boxSize( (*it).v3, min, max, x, y, z, TOLERANCE);
	}
	DBGP("Max: " << max);
	DBGP("Min: " << min);
	for (int i=0; i<3; i++) {
		halfSize[i] = 0.5 * (max[i] - min[i]);
	}
	DBGP("computed halfsize: " << halfSize);
	//halfSize = 0.5 * (max - min);
	center = min + halfSize;
	center = R.inverse() * center;
	//sanity check
	for (int i=0; i<3; i++) {
		if (halfSize[i] < TOLERANCE) {
			if (halfSize[i] < 0.5 * TOLERANCE) {
				DBGA("Warning: degenerate box computed");
			}
			halfSize[i] = TOLERANCE;
		}
	}
	DBGP("returned halfsize: " << halfSize);
}
void GuiDecalEditorCtrl::renderScene(const RectI & updateRect)
{
   PROFILE_SCOPE( GuiDecalEditorCtrl_renderScene );

   GFXTransformSaver saver;

   RectI bounds = getBounds();
   
   ColorI hlColor(0,255,0,255);
   ColorI regColor(255,0,0,255);
   ColorI selColor(0,0,255,255);
   ColorI color;
   
   GFXDrawUtil *drawUtil = GFX->getDrawUtil();   

   GFXStateBlockDesc desc;
   desc.setBlend( true );
   desc.setZReadWrite( true, false );

   // Draw 3D stuff here.   
   if ( mSELDecal )
   {
      mGizmo->renderGizmo( mLastCameraQuery.cameraMatrix, mLastCameraQuery.fov );

      mSELEdgeVerts.clear();
      if ( gDecalManager->clipDecal( mSELDecal, &mSELEdgeVerts ) )
         _renderDecalEdge( mSELEdgeVerts, ColorI( 255, 255, 255, 255 ) );

      const F32 &decalSize = mSELDecal->mSize;
      Point3F boxSize( decalSize, decalSize, decalSize );

      MatrixF worldMat( true );
      mSELDecal->getWorldMatrix( &worldMat, true );   

      drawUtil->drawObjectBox( desc, boxSize, mSELDecal->mPosition, worldMat, ColorI( 255, 255, 255, 255 ) );
   }

   if ( mHLDecal )
   {
      mHLEdgeVerts.clear();
      if ( gDecalManager->clipDecal( mHLDecal, &mHLEdgeVerts ) )
         _renderDecalEdge( mHLEdgeVerts, ColorI( 255, 255, 255, 255 ) );

      const F32 &decalSize = mHLDecal->mSize;
      Point3F boxSize( decalSize, decalSize, decalSize );

      MatrixF worldMat( true );
      mHLDecal->getWorldMatrix( &worldMat, true );  

      drawUtil->drawObjectBox( desc, boxSize, mHLDecal->mPosition, worldMat, ColorI( 255, 255, 255, 255 ) );
   }
} 
Beispiel #3
0
void GameLayer::importGroundData(cocos2d::TMXTiledMap* data)
{
	TMXObjectGroup *objects = m_pTiledMap->getObjectGroup("Grounds");
	const ValueVector _objects = objects->getObjects();
	if (!_objects.empty())
	{
		for (const auto& v : _objects)
		{
			const ValueMap& dict = v.asValueMap();
			if (dict.find("name") != dict.end())
			{
				if (dict.at("name").asString() == "Ground" || dict.at("name").asString() == "Box")
				{
					Size boxSize(dict.at("width").asFloat(), dict.at("height").asFloat());
					auto ground = Ground::create();
					if(dict.find("rotation") != dict.end())
						ground->initPhysics(boxSize, Point(dict.at("x").asFloat(), dict.at("y").asFloat()), dict.at("rotation").asInt());
					else
						ground->initPhysics(boxSize, Point(dict.at("x").asFloat(), dict.at("y").asFloat()), 0);
					this->addChild(ground);
				}
			}
		}
	}
}
Beispiel #4
0
std::string Context::describe() {
    namespace rus = readdy::util::str;
    configure();
    std::string description;
    description += fmt::format("Configured kernel context with:{}", rus::newline);
    description += fmt::format("--------------------------------{}", rus::newline);
    description += fmt::format(" - kBT = {}{}", kBT(), rus::newline);
    description += fmt::format(" - periodic b.c. = ({}, {}, {}){}",
                               periodicBoundaryConditions()[0],
                               periodicBoundaryConditions()[1],
                               periodicBoundaryConditions()[2],
                               rus::newline);
    description += fmt::format(" - box size = ({}, {}, {}){}", boxSize()[0], boxSize()[1], boxSize()[2], rus::newline);

    description += _particleTypeRegistry.describe();
    description += _potentialRegistry.describe();
    description += _reactionRegistry.describe();
    description += _topologyRegistry.describe();
    return description;
}
Beispiel #5
0
  /// Main worker function (entry method).
  /// In this simple example, an element performs the following:
  /// 1. Create a bunch of boxes from this element.  These are all
  ///    in the x-y plane and all parallel to the y-axis.
  /// 2. Stretch the first box over into next object.
  ///    With this, the first box on elment 'e' will overlap with the first
  ///    and second box on element 'e+1'
  /// 3. Send these objects to be collided. Results go to the collide manager
  ///    group with which this chunk (element) was registered at construction. 
  ///    This is done by invoking the method 'contribute' on the local branch
  ///    of the collide manager group, passing it the chunk (element) number,
  ///    the number of boxes created by this element, the actual boxes, and
  ///    the array of priority flags.
  ///
  /// Setting priority=NULL here will use the default setting which is
  /// priority=thisIndex.  In other words, objects created in the same
  /// chunk (i.e. by the same element) will not collide with each other.
  ///
  /// \see CollideBoxesPrio defined in collidecharm.{h,C}
  ///
  void DoIt(void)
  {
    CkPrintf("Contributing to reduction %d, element %04d\n", nTimes, thisIndex);

    // Create a bunch of boxes from this element.

    ////CkVector3d o(-6.8, 7.9, 8.0);
    CkVector3d o( 0,   0,   0);
    CkVector3d x( 4,   0,   0);
    CkVector3d y( 0,   0.3, 0);
    CkVector3d boxSize(0.2, 0.2, 0.2);
    ////int nBoxes = 1000;
    int nBoxes = 10;
    bbox3d *box = new bbox3d[nBoxes];
    for (int i=0; i<nBoxes; i++) {
      CkVector3d c(o + x*thisIndex + y*i);
      CkVector3d c2(c + boxSize);
      // Create a box with corners 'c' and 'c2'
      box[i].empty();
      box[i].add(c); box[i].add(c2);
      
#ifdef RADU_DBG
      // Write box information
      rSeg1d x_dim = box[i].axis(0);
      rSeg1d y_dim = box[i].axis(1);
      rSeg1d z_dim = box[i].axis(2);
      CkPrintf("%d:%d   (%g, %g, %g) - (%g, %g, %g)\n",
               thisIndex, i,
               x_dim.getMin(), y_dim.getMin(), z_dim.getMin(),
               x_dim.getMax(), y_dim.getMax(), z_dim.getMax());
#endif
    } 
    
    // Stretch the first box over into next object.
    box[0].add(o + x*(thisIndex+1.5) + y*2);

#ifdef RADU_DBG	
    // Stretched box[0]
    rSeg1d x_dim = box[0].axis(0);
    rSeg1d y_dim = box[0].axis(1);
    rSeg1d z_dim = box[0].axis(2);
    CkPrintf(">>> %d:0   (%g, %g, %g) - (%g, %g, %g)\n",
             thisIndex,
             x_dim.getMin(), y_dim.getMin(), z_dim.getMin(),
             x_dim.getMax(), y_dim.getMax(), z_dim.getMax());
#endif
    
    // Send these objects to be collided.
    CollideBoxesPrio(collide, thisIndex, nBoxes, box, NULL);
    
    delete[] box;
    nTimes++;
  }
Beispiel #6
0
void	BenchmarkDemo::createTest6()
{
	setCameraDistance(btScalar(250.));

	btVector3 boxSize(1.5f,1.5f,1.5f);

	btConvexHullShape* convexHullShape = new btConvexHullShape();

	for (int i=0;i<TaruVtxCount;i++)
	{
		btVector3 vtx(TaruVtx[i*3],TaruVtx[i*3+1],TaruVtx[i*3+2]);
		convexHullShape->addPoint(vtx);
	}

	btTransform trans;
	trans.setIdentity();

	float mass = 1.f;
	btVector3 localInertia(0,0,0);
	convexHullShape->calculateLocalInertia(mass,localInertia);


	{
		int size = 10;
		int height = 10;

		const float cubeSize = boxSize[0];
		float spacing = 2.0f;
		btVector3 pos(0.0f, 20.0f, 0.0f);
		float offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;
		
	
		for(int k=0;k<height;k++) {
			for(int j=0;j<size;j++) {
				pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
				for(int i=0;i<size;i++) {
					pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
					btVector3 bpos = btVector3(0,25,0) + btVector3(5.0f,1.0f,5.0f)*pos;
					trans.setOrigin(bpos);
					
					localCreateRigidBody(mass,trans,convexHullShape);
				}
			}
			offset -= 0.05f * spacing * (size-1);
			spacing *= 1.1f;
			pos[1] += (cubeSize * 2.0f + spacing);
		}
	}


	createLargeMeshBody();
}
/*! The split is done along the given x axis, and the optimal child bounding boxes
	are aligned with the x,y,z axes. Of course, there is no guarantee that the 
	children will actually build their own bboxes along those axes; as they will
	do their own principal components they might end up with boxes aligned
	differently.
*/
void 
Leaf::optimalSplit(const vec3 &x, const vec3 &y, const vec3 &z, Leaf *child1, Leaf *child2)
{
	//sort triangles by centroid projections
	std::vector< std::pair<Triangle,double> > sortedTriangles;
	std::list<Triangle>::iterator it;
	for (it=mTriangles.begin(); it!=mTriangles.end(); it++) {
		position median = (*it).v1 + (*it).v2 + (*it).v3;
		median =  ( 1.0 / 3.0 ) * median;
		double projection = (median - position::ORIGIN) % x;
		sortedTriangles.push_back( std::pair<Triangle,double>(*it,projection) );
	}
	std::sort(sortedTriangles.begin(), sortedTriangles.end(), compareProjections);

	//compute bbox volumes going up
	std::vector<double> volumesUp;
	vec3 max(-1.0e10, -1.0e10, -1.0e10);
	vec3 min( 1.0e10,  1.0e10,  1.0e10);
	std::vector< std::pair<Triangle,double> >::iterator it2;
	for (it2 = sortedTriangles.begin(); it2!=sortedTriangles.end(); it2++) {
		boxSize((*it2).first.v1, min, max, x, y, z, Leaf::TOLERANCE);
		boxSize((*it2).first.v2, min, max, x, y, z, Leaf::TOLERANCE);
		boxSize((*it2).first.v3, min, max, x, y, z, Leaf::TOLERANCE);
		double volumeSq = (max[0] - min[0]) * (max[1] - min[1]) * (max[2] - min[2]);
		volumesUp.push_back(volumeSq);
	}
	//and bbox volumes going down
	std::vector<double> volumesDown;
	max.set(-1.0e10, -1.0e10, -1.0e10);
	min.set( 1.0e10,  1.0e10,  1.0e10);
	std::vector< std::pair<Triangle,double> >::reverse_iterator it3;
	for (it3 = sortedTriangles.rbegin(); it3!=sortedTriangles.rend(); it3++) {
		boxSize((*it3).first.v1, min, max, x, y, z, Leaf::TOLERANCE);
		boxSize((*it3).first.v2, min, max, x, y, z, Leaf::TOLERANCE);
		boxSize((*it3).first.v3, min, max, x, y, z, Leaf::TOLERANCE);
		double volumeSq = (max[0] - min[0]) * (max[1] - min[1]) * (max[2] - min[2]);
		volumesDown.push_back(volumeSq);
	}
	assert( volumesUp.size() == volumesDown.size() );

	//find optimal split point
	for (int i=0; i<(int)volumesUp.size(); i++) {
		volumesUp[i] += volumesDown[ volumesDown.size()-i-1 ];
	}
	std::vector<double>::iterator minVolIt = std::min_element(volumesUp.begin(), volumesUp.end());

	//assign triangles to children
	std::vector<double>::iterator it4;
	int index=0;
	for (it4 = volumesUp.begin(); it4 != minVolIt; it4++) {
		child1->addTriangle(sortedTriangles[index].first);
		index++;
	}
	for (it4 = minVolIt; it4 != volumesUp.end(); it4++) {
		child2->addTriangle(sortedTriangles[index].first);
		index++;
	}
}
Beispiel #8
0
/// get the intersected piece from a ray in world space
int  cRubikSnake::GetRayIntersection( TSRVector3& raySource, TSRVector3& rayDirection )
{
    float Dist = FLT_MAX;
    float newDist = 0.0f;

    TSRMatrix4 currTransform;
    currTransform.MakeIdent();

    int iPossibleMoveIndex = -1;

    float dummyT;
    TSRVector3 q;


    TSRMatrixStack stack;
    stack.LoadIdentity();
    stack.Push();

    stack.TranslateLocal( m_vPanTranslation.x, m_vPanTranslation.y, m_vPanTranslation.z );
    stack.MultMatrix( m_ArcBall.m_Transform.d );
    stack.TranslateLocal( -m_Center.x, -m_Center.y, -m_Center.z );
    
    TSRVector3 rotAxis( 1.0f, 0.0f, 0.0f );
    
    for ( unsigned int i = 0; i < m_Angles.size(); i++ )
    {
        stack.Push();
        stack.TranslateLocal( 0.25f, 0.25f, 0.0f );
        memcpy( currTransform.d, stack.GetTop(), sizeof( TSRMatrix4 ) );
        stack.Pop();
        TSRVector3 boxSize( 0.6f, 0.6f, 0.6f );
        stack.MultMatrix( m_Transform.d );
        if ( TSRCollisionTests::IntersectRayOBB( raySource, rayDirection, currTransform, boxSize, dummyT, q ) )
        {
            newDist = ( raySource - q ).Mag();
            if ( newDist < Dist )
            {
                Dist = newDist; 
                iPossibleMoveIndex = i;
                m_SelectedIndex = i;
            }
        }
        stack.TranslateLocal( -0.5f, 0.5f, 0.0f );
        stack.RotateAxisLocal( rotAxis, m_Angles[ i ] * PI / 180.0f );
    }
    stack.Pop();
	return iPossibleMoveIndex;
}
Beispiel #9
0
void GridDisplay::drawGrid() {
	int width = round((double)this->matSize.width/this->gridSize.width);
	int height = round((double)this->matSize.height/this->gridSize.height);
	Size boxSize(width,height);
	Point endPt;
	int row=0, col=0;
	while(row<this->img.rows) {
		while(col<this->img.cols) {
			endPt.x = min(col+boxSize.width-1,img.cols-1);
			endPt.y = min(row+boxSize.height-1,img.rows-1);
			rectangle(this->img,Point(col,row),endPt,Scalar(100));
			col = endPt.x+1;
		}
		col=0;
		row = endPt.y+1;
	}
}
Beispiel #10
0
int main(int argc, char **argv){
  int peid, numpes;
  MPI_Comm newComm;

  //basic MPI initilization
  MPI_Init(&argc, &argv);
  MPI_Comm_rank(MPI_COMM_WORLD, &peid);
  MPI_Comm_size(MPI_COMM_WORLD, &numpes);


  //initialize Charm for each set
  CharmLibInit(MPI_COMM_WORLD, argc, argv);
  MPI_Barrier(MPI_COMM_WORLD);
  CollisionList *colls;
  CkVector3d o(-6.8,7.9,8.0), x(4.0,0,0), y(0,0.3,0);
  CkVector3d boxSize(0.2,0.2,0.2);
  int nBoxes=1000;
  bbox3d *box=new bbox3d[nBoxes];
  for (int i=0;i<nBoxes;i++) {
	  CkVector3d c(o+x*peid+y*i);
	  CkVector3d c2(c+boxSize);
	  box[i].empty();
	  box[i].add(c); box[i].add(c2);
  }
  // first box stretches over into next object:
  box[0].add(o+x*(peid+1.5)+y*2);
  detectCollision(colls,nBoxes, box, NULL);
  int numColls=colls->length();
  for (int c=0;c<numColls;c++) {
	  printf("%d:%d hits %d:%d\n",
			  (*colls)[c].A.chunk,(*colls)[c].A.number,
			  (*colls)[c].B.chunk,(*colls)[c].B.number);
  }

  delete box;
  MPI_Barrier(MPI_COMM_WORLD);
  CharmLibExit();

  //final synchronization
  MPI_Barrier(MPI_COMM_WORLD);

  MPI_Finalize();
  return 0;  
}
void createSceneJoints()
{
	
	const int n = 10;

	int startId = numRigidBodies;

	PfxVector3 boxSize(1.0f);
	PfxFloat boxMass = 1.0f;

	for(int i=0;i<n;i++) {
		createBrick(numRigidBodies++,PfxVector3(0,3.0f+i*2.5f*boxSize[1],0),PfxQuat::identity(),boxSize,boxMass);
	}

	for(int i=startId;i<startId+n;i++) {
		PfxRigidState &stateA = states[i];
		PfxRigidState &stateB = states[(i+1)%numRigidBodies];
		PfxVector3 anchor;
		if(i == numRigidBodies-1) {
			anchor = stateA.getPosition() + PfxVector3(0,boxSize[1],0);
		}
		else {
			anchor = ( stateA.getPosition() + stateB.getPosition() ) * 0.5f;
		}

		PfxSwingTwistJointInitParam jparam;
		jparam.anchorPoint = anchor;
		jparam.twistAxis = PfxVector3(0,1,0);

		pfxInitializeSwingTwistJoint(joints[numJoints],stateA,stateB,jparam);
		joints[numJoints].m_constraints[4].m_damping = 0.1f;
		joints[numJoints].m_constraints[5].m_damping = 0.1f;


		SCE_PFX_ASSERT(numJoints<NUM_JOINTS);
		numJoints++;
	}

	states[startId].setLinearVelocity(PfxVector3(0,0,5));
	states[startId].setLinearDamping(0.95f);
	states[startId].setAngularDamping(0.95f);
	
}
Beispiel #12
0
GLFont::Box GLFont::calcStringBox(GLsizei stringWidth) const
	{
	/* Calculate the string's scaled width: */
	Vector boxSize(GLfloat(stringWidth-1)*textHeight/GLfloat(fontHeight-1),textHeight,0.0f);
	
	/* Calculate the string's bounding box origin: */
	Vector boxOrigin(0.0f,0.0f,0.0f);
	switch(hAlignment)
		{
		case Left:
			boxOrigin[0]=0.0f;
			break;
		
		case Center:
			boxOrigin[0]=-0.5f*boxSize[0];
			break;
		
		case Right:
			boxOrigin[0]=-boxSize[0];
			break;
		}
	switch(vAlignment)
		{
		case Top:
			boxOrigin[1]=-boxSize[1];
			break;
		
		case VCenter:
			boxOrigin[1]=-0.5f*boxSize[1];
			break;
		
		case Baseline:
			boxOrigin[1]=-boxSize[1]*GLfloat(baseLine)/GLfloat(fontHeight);
			break;
		
		case Bottom:
			boxOrigin[1]=0.0f;
			break;
		}
	
	return Box(boxOrigin,boxSize);
	}
QRectF PipelineFlowChart::boxRect(int i)
{
  QRectF totalRect = totalAreaRect();

  qreal boxeswidth = totalRect.width() - numGaps() * boxMargin();

  qreal boxdim = qMin(totalRect.height(), boxeswidth / numItems());

  boxdim = qMax(MinBoxDimension, boxdim);

  qreal oblongwidth = qMax(0.0, (boxeswidth - boxdim * numItems()) / numItems());

  QSizeF boxSize(boxdim + oblongwidth, boxdim);

  QRectF ret(totalRect.x() + i * (boxSize.width() + boxMargin()),
             totalRect.y() + totalRect.height() / 2 - boxSize.height() / 2, boxSize.width(),
             boxSize.height());

  return ret;
}
Beispiel #14
0
void DemoKeeper::BrickWallDemo( void )
{
	mWorld->markForWrite();

	//
	// Create the walls
	//

	hkVector4 groundPos( 0.0f, 0.0f, 0.0f );
	hkVector4 boxSize( 1.0f, 1.0f, 1.0f);
	hkReal deltaZ = 25.0f;
	groundPos(2) = -deltaZ * 10 * 0.5f;

	for ( int y = 0; y < 4; y ++ )			// first wall
	{
		createBrickWall( 4, 8, groundPos, 0.2f, boxSize );
		groundPos(2) += deltaZ;
	}

	mWorld->unmarkForWrite();
}
Beispiel #15
0
  void DoIt(void)
  {
	CkPrintf("Contributing to reduction %d, element %04d\n",nTimes,thisIndex);
	CkVector3d o(-6.8,7.9,8.0), x(4.0,0,0), y(0,0.3,0);
	CkVector3d boxSize(0.2,0.2,0.2);
	int nBoxes=1000;
	bbox3d *box=new bbox3d[nBoxes];
	for (int i=0;i<nBoxes;i++) {
		CkVector3d c(o+x*thisIndex+y*i);
		CkVector3d c2(c+boxSize);
		box[i].empty();
		box[i].add(c); box[i].add(c2);
	} 
	// first box stretches over into next object:
	box[0].add(o+x*(thisIndex+1.5)+y*2);
	
	CollideBoxesPrio(collide,thisIndex,nBoxes,box,NULL);
	
	delete[] box;
	nTimes++;
  }
Beispiel #16
0
void Player::update(float dt)
{
    Rect rect = m_model->realBoundingBoxForCurrentFrame();
    Vec3 scale, pos; Quaternion rot;
    getNodeToWorldTransform().decompose(&scale, &rot, &pos);

    do
    {
        Node* level = Director::getInstance()->getRunningScene()->getChildByTag(1);
        if (level == nullptr)
        {
            break;
        }
        Vec2 position = level->getPosition();
        if (m_state == EWalkLeft)
        {
            position += dt * m_speed * Vec2(scale.x, 0);
        }
        else if (m_state == EWalkRight)
        {
            position += dt * m_speed * Vec2(-scale.x, 0);
        }
        else
        {
            break;
        }
        level->setPosition(position);

    } while (0);

    Size boxSize(rect.size.width * scale.x, rect.size.height * scale.y);
    Vec2 boxPos(rect.origin.x * scale.x + boxSize.width / 2, rect.origin.y * scale.y + boxSize.height / 2);
    auto body = PhysicsBody::createBox(boxSize, PHYSICSBODY_MATERIAL_DEFAULT);
    body->setPositionOffset(boxPos);
    setPhysicsBody(body);
    body->setContactTestBitmask(0x2);

}
Beispiel #17
0
void Box::apply_plane_collision(float time, Map* ptr) {
  collision = false;
  sf::Vector2f boxSize(rectW,rectL);
  sf::Vector2f mapSize(800,5);

  sf::Vector2f planePos = ptr->getPlanePosition();
  sf::Vector2f boxPos = rect.getPosition();

  sf::FloatRect planeRec(planePos, mapSize );
  sf::FloatRect boxRec(boxPos,boxSize);

  sf::Vector2f distance = planePos - boxPos;
  float dist = sqrt(pow(distance.x,2) + pow(distance.y,2) );

  if( dist < 2*rectW) {
    if( boxRec.intersects(planeRec) ){
      if(!collision) {
	collision = true;
	bounceNumber++;
	speed = -speed;
      }
    }
  }
}
hkpRigidBody* PairCollisionFilter::createMovingBox() {
	hkVector4 boxSize(.5f, .5f, .5f);
	hkpShape* boxShape = new hkpBoxShape(boxSize, 0);

	hkpRigidBodyCinfo info;
	info.m_shape = boxShape;
	info.m_qualityType = HK_COLLIDABLE_QUALITY_MOVING;

	hkReal boxMass = 1.f;
	hkMassProperties massProperties;
	hkpInertiaTensorComputer::computeBoxVolumeMassProperties(boxSize, boxMass, massProperties);
	info.m_motionType = hkpMotion::MOTION_BOX_INERTIA;
	info.m_mass = boxMass;
	info.m_inertiaTensor = massProperties.m_inertiaTensor;
	info.m_position.set(0.f, 7.f, 0.f);

	hkpRigidBody* boxBody = new hkpRigidBody(info);
	m_world->addEntity(boxBody);

	boxBody->removeReference();
	boxShape->removeReference();

	return boxBody;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale)
{
    dgAssert(proxy.m_referenceCollision->IsType(dgCollision::dgCollisionConvexShape_RTTI));
    dgAssert(proxy.m_floatingCollision->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));

    const dgCollisionInstance* const polygonInstance = proxy.m_floatingCollision;
    dgAssert(this == polygonInstance->GetChildShape());
    dgAssert(m_count);
    dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));

    dgInt32 count = 0;


    m_normal = m_normal.CompProduct4(polyInstanceInvScale);
    dgAssert(m_normal.m_w == dgFloat32(0.0f));
    m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt());
    dgVector savedFaceNormal(m_normal);

    dgVector savedPosit (proxy.m_matrix.m_posit);
    proxy.m_matrix.m_posit = dgVector::m_wOne;

    dgVector hullOrigin(proxy.m_matrix.UnrotateVector (savedPosit));
    for (dgInt32 i = 0; i < m_count; i++) {
        m_localPoly[i] = hullOrigin + polyInstanceScale.CompProduct4(dgVector(&m_vertex[m_vertexIndex[i] * m_stride]));
        dgAssert(m_localPoly[i].m_w == dgFloat32(0.0f));
    }

    dgContact* const contactJoint = proxy.m_contactJoint;
    const dgCollisionInstance* const hull = proxy.m_referenceCollision;

    dgVector normalInHull(proxy.m_matrix.RotateVector(m_normal));
    dgVector pointInHull(hull->SupportVertex(normalInHull.Scale4(dgFloat32(-1.0f)), NULL));
    dgVector p0(proxy.m_matrix.UntransformVector(pointInHull));
    dgVector p1(proxy.m_matrix.UntransformVector(hull->SupportVertex(normalInHull, NULL)));

    dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
    if (penetration < dgFloat32(0.0f)) {
        contactJoint->m_closestDistance = -penetration;
        proxy.m_matrix.m_posit = savedPosit;
        return 0;
    }

    contactJoint->m_closestDistance = dgFloat32(0.0f);
    dgFloat32 distance = (m_localPoly[0] - p1) % m_normal;
    if (distance >= dgFloat32(0.0f)) {
        proxy.m_matrix.m_posit = savedPosit;
        return 0;
    }

    dgVector boxSize (hull->GetBoxSize() & dgVector::m_triplexMask);
    dgVector boxOrigin ((hull->GetBoxOrigin() & dgVector::m_triplexMask) + dgVector::m_wOne);

    bool inside = true;
    dgInt32 i0 = m_count - 1;
    for (dgInt32 i = 0; i < m_count; i++) {

        dgVector e(m_localPoly[i] - m_localPoly[i0]);
        dgVector n(m_normal * e);
        //dgPlane plane(n, -(m_localPoly[i0] % n));
        dgPlane plane(n, - m_localPoly[i0].DotProduct4 (n).GetScalar());
        plane = proxy.m_matrix.TransformPlane(plane);

        //dgFloat32 supportDist = dgAbsf(plane.m_x) * boxSize.m_x + dgAbsf(plane.m_y) * boxSize.m_y + dgAbsf(plane.m_z) * boxSize.m_z;
        //dgFloat32 centerDist = plane.Evalue(boxOrigin);
        dgFloat32 supportDist = boxSize.DotProduct4 (plane.Abs()).GetScalar();
        dgFloat32 centerDist = plane.DotProduct4 (boxOrigin).GetScalar();

        if ((centerDist + supportDist) < dgFloat32(0.0f)) {
            proxy.m_matrix.m_posit = savedPosit;
            return 0;
        }

        if ((centerDist - supportDist) < dgFloat32(0.0f)) {
            inside = false;
            break;
        }
        i0 = i;
    }

    const dgInt32 hullId = hull->GetUserDataID();
    if (inside & !proxy.m_intersectionTestOnly) {
        dgAssert(penetration >= dgFloat32(0.0f));
        dgVector pointsContacts[64];

        dgAssert(penetration >= 0.0f);
        dgVector point(pointInHull + normalInHull.Scale4(penetration));

        count = hull->CalculatePlaneIntersection(normalInHull.Scale4(dgFloat32(-1.0f)), point, pointsContacts, 1.0f);
        dgVector step(normalInHull.Scale4((proxy.m_skinThickness - penetration) * dgFloat32(0.5f)));

        const dgMatrix& worldMatrix = hull->m_globalMatrix;
        dgContactPoint* const contactsOut = proxy.m_contacts;
        dgAssert(contactsOut);
        dgVector globalNormal(worldMatrix.RotateVector(normalInHull));
        for (dgInt32 i = 0; i < count; i++) {
            contactsOut[i].m_point = worldMatrix.TransformVector(pointsContacts[i] + step);
            contactsOut[i].m_normal = globalNormal;
            contactsOut[i].m_shapeId0 = hullId;
            contactsOut[i].m_shapeId1 = m_faceId;
            contactsOut[i].m_penetration = penetration;
        }
    } else {
        dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize();
        if (m_faceClipSize > convexSphapeUmbra) {
            BeamClipping(dgVector(dgFloat32(0.0f)), convexSphapeUmbra);
            m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
        }

        dgCollisionConvex* const convexShape = (dgCollisionConvex*)hull->m_childShape;
        count = convexShape->CalculateConvexToConvexContact(proxy);
        dgAssert(proxy.m_intersectionTestOnly || (count >= 0));
        if (count >= 1) {
            dgContactPoint* const contactsOut = proxy.m_contacts;
            if (m_closestFeatureType == 3) {
                for (dgInt32 i = 0; i < count; i++) {
                    //contactsOut[i].m_userId = m_faceId;
                    contactsOut[i].m_shapeId0 = hullId;
                    contactsOut[i].m_shapeId1 = m_faceId;
                }
            } else {
                dgVector normal(polygonInstance->m_globalMatrix.UnrotateVector(contactsOut[0].m_normal));
                if (normal.DotProduct4(savedFaceNormal).GetScalar() < dgFloat32(0.9995f)) {
                    dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
                    dgVector n(&m_vertex[index * m_stride]);
                    if ((savedFaceNormal.DotProduct4(n).GetScalar() > dgFloat32(0.9995f))) {
                        normal = n;
                    } else {
                        dgVector dir0(n * savedFaceNormal);
                        dgVector dir1(n * normal);
                        dgFloat32 projection = dir0.DotProduct4(dir1).GetScalar();
                        if (projection <= dgFloat32(0.0f)) {
                            normal = n;
                        }
                    }
                    normal = polygonInstance->m_globalMatrix.RotateVector(normal);

                    for (dgInt32 i = 0; i < count; i++) {
                        contactsOut[i].m_normal = normal;
                        //contactsOut[i].m_userId = m_faceId;
                        contactsOut[i].m_shapeId0 = hullId;
                        contactsOut[i].m_shapeId1 = m_faceId;
                    }
                } else {
                    for (dgInt32 i = 0; i < count; i++) {
                        //contactsOut[i].m_userId = m_faceId;
                        contactsOut[i].m_shapeId0 = hullId;
                        contactsOut[i].m_shapeId1 = m_faceId;
                    }
                }
            }
        }
    }

    proxy.m_matrix.m_posit = savedPosit;
    return count;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue (dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale)
{
    dgAssert (proxy.m_referenceCollision->IsType (dgCollision::dgCollisionConvexShape_RTTI));
    dgAssert (proxy.m_floatingCollision->IsType (dgCollision::dgCollisionConvexPolygon_RTTI));

    const dgCollisionInstance* const hull = proxy.m_referenceCollision;

    dgAssert (this == proxy.m_floatingCollision->GetChildShape());
    dgAssert (m_count);
    dgAssert (m_count < dgInt32 (sizeof (m_localPoly) / sizeof (m_localPoly[0])));

    const dgBody* const floatingBody = proxy.m_floatingBody;
    const dgBody* const referenceBody = proxy.m_referenceBody;

    dgContact* const contactJoint = proxy.m_contactJoint;
    contactJoint->m_closestDistance = dgFloat32 (1.0e10f);

    m_normal = m_normal.CompProduct4(polyInstanceInvScale);
    dgAssert (m_normal.m_w == dgFloat32 (0.0f));
    m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt());
    const dgVector savedFaceNormal (m_normal);

    for (dgInt32 i = 0; i < m_count; i ++) {
        m_localPoly[i] = polyInstanceScale.CompProduct4(dgVector (&m_vertex[m_vertexIndex[i] * m_stride]));
        dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
    }

    dgVector hullOrigin (proxy.m_matrix.UntransformVector(dgVector (dgFloat32 (0.0f))));
    hullOrigin = (hullOrigin - m_normal.CompProduct4(m_normal.DotProduct4(hullOrigin - m_localPoly[0]))) | dgVector::m_wOne;

    dgMatrix polygonMatrix;
    polygonMatrix[0] = m_localPoly[1] - m_localPoly[0];
    polygonMatrix[0] = polygonMatrix[0].CompProduct4 (polygonMatrix[0].InvMagSqrt());
    polygonMatrix[1] = m_normal;
    polygonMatrix[2] = polygonMatrix[0] * m_normal;
    polygonMatrix[3] = hullOrigin;
    dgAssert (polygonMatrix.TestOrthogonal());

    dgMatrix savedProxyMatrix (proxy.m_matrix);
    proxy.m_matrix = polygonMatrix * proxy.m_matrix;

    dgVector floatingVeloc (floatingBody->m_veloc);
    dgVector referenceVeloc (referenceBody->m_veloc);
    const dgMatrix& hullMatrix = hull->GetGlobalMatrix();
    dgVector hullRelativeVeloc (hullMatrix.UnrotateVector(referenceVeloc - floatingVeloc));
    dgVector polyRelativeVeloc (proxy.m_matrix.UnrotateVector (hullRelativeVeloc));

    dgVector polyBoxP0 (dgFloat32 ( 1.0e15f));
    dgVector polyBoxP1 (dgFloat32 (-1.0e15f));
    m_normal = polygonMatrix.UnrotateVector(m_normal);

    if (m_normal.DotProduct4(polyRelativeVeloc).m_x >= 0.0f) {
        proxy.m_matrix = savedProxyMatrix;
        return 0;
    }
    for (dgInt32 i = 0; i < m_count; i ++) {
        m_localPoly[i] = polygonMatrix.UntransformVector(m_localPoly[i]);
        dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
        polyBoxP0 = polyBoxP0.GetMin (m_localPoly[i]);
        polyBoxP1 = polyBoxP1.GetMax (m_localPoly[i]);
    }
    dgInt32 count = 0;


    dgVector hullBoxP0;
    dgVector hullBoxP1;
    hull->CalcAABB (proxy.m_matrix.Inverse(), hullBoxP0, hullBoxP1);
    dgVector minBox (polyBoxP0 - hullBoxP1);
    dgVector maxBox (polyBoxP1 - hullBoxP0);
    dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), polyRelativeVeloc);
    dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);

    if (distance < dgFloat32 (1.0f)) {

        dgVector boxSize ((hullBoxP1 - hullBoxP0).Scale4 (dgFloat32 (0.5f)));
//		dgVector boxOrigin ((hullBoxP1 + hullBoxP0).Scale4 (dgFloat32 (0.5f)));
//		boxOrigin += polyRelativeVeloc.Scale4 (distance);

        dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f))));
        dgVector pointInHull (hull->SupportVertex (normalInHull, NULL));
        dgVector pointInPlane (proxy.m_matrix.UntransformVector (pointInHull));
        dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;
        dgFloat32 timeToPlane = distToPlane / (polyRelativeVeloc % m_normal);
        dgVector boxOrigin (pointInPlane + polyRelativeVeloc.Scale4(timeToPlane));

        bool inside = true;
        dgInt32 i0 = m_count - 1;
        for (dgInt32 i = 0; i < m_count; i ++) {
            dgVector e (m_localPoly[i] - m_localPoly[i0]);
            dgVector n (m_normal * e);
            dgPlane plane (n, - (m_localPoly[i0] % n));

            dgVector supportDist (plane.Abs().DotProduct4 (boxSize));
            dgFloat32 centerDist = plane.Evalue(boxOrigin);

            if ((centerDist + supportDist.m_x) < dgFloat32 (0.0f)) {
                proxy.m_matrix = savedProxyMatrix;
                return 0;
            }

            if ((centerDist - supportDist.m_x) < dgFloat32 (0.0f)) {
                inside = false;
            }
            i0 = i;
        }

// for the time being for the minkousky contact calculation
        inside = false;
        const dgInt32 hullId = hull->GetUserDataID();
        if (inside) {
            dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f))));
            dgVector pointInHull (hull->SupportVertex (normalInHull, NULL));
            dgVector p0 (proxy.m_matrix.UntransformVector (pointInHull));

            dgFloat32 timetoImpact = dgFloat32 (0.0f);
            //dgFloat32 closestDistance = dgFloat32 (0.0f);
            dgAssert (0);
//			dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness + DG_IMPULSIVE_CONTACT_PENETRATION;
            dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
            if (penetration < dgFloat32 (0.0f)) {
                timetoImpact = penetration / (polyRelativeVeloc % m_normal);
                dgAssert (timetoImpact >= dgFloat32 (0.0f));
//				closestDistance = -penetration;
            }

            if (timetoImpact <= proxy.m_timestep) {
                dgVector pointsContacts[64];

                contactJoint->m_closestDistance = penetration;
                dgAssert (0);
//				dgVector point (pointInHull - normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION));
                dgVector point (pointInHull);

                count = hull->CalculatePlaneIntersection (normalInHull, point, pointsContacts, 1.0f);
                dgAssert (0);
//				dgVector step (hullRelativeVeloc.Scale3 (timetoImpact) + normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION));
                dgVector step (hullRelativeVeloc.Scale3 (timetoImpact));

                penetration = dgMax (penetration, dgFloat32 (0.0f));
                const dgMatrix& worldMatrix = hull->m_globalMatrix;
                dgContactPoint* const contactsOut = proxy.m_contacts;
                dgVector globalNormal (worldMatrix.RotateVector(normalInHull));
                for (dgInt32 i = 0; i < count; i ++) {
                    contactsOut[i].m_point = worldMatrix.TransformVector (pointsContacts[i] + step);
                    contactsOut[i].m_normal = globalNormal;
                    contactsOut[i].m_shapeId0 = hullId;
                    contactsOut[i].m_shapeId1 = m_faceId;
                    contactsOut[i].m_penetration = penetration;
                }
            }
        } else {
            dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize ();
            if (m_faceClipSize > convexSphapeUmbra) {
                BeamClipping (boxOrigin, convexSphapeUmbra);
                m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
            }

            dgCollisionConvex* const convexShape = (dgCollisionConvex*) hull->m_childShape;
            count = convexShape->CalculateConvexCastContacts (proxy);

//			dgAssert (proxy.m_intersectionTestOnly || (count >= 0));
            if (count >= 1) {
                dgContactPoint* const contactsOut = proxy.m_contacts;
#if 0
                if (m_closestFeatureType == 3) {
                    for (dgInt32 i = 0; i < count; i ++) {
                        contactsOut[i].m_shapeId0 = hullId;
                        contactsOut[i].m_shapeId1 = m_faceId;
                    }
                } else {
                    dgVector normal (polygonInstance->m_globalMatrix.UnrotateVector(contactsOut[0].m_normal));
                    if ((normal % savedFaceNormal) < dgFloat32 (0.995f)) {
                        dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
                        dgVector n (&m_vertex[index * m_stride]);
                        dgVector dir0 (n * savedFaceNormal);
                        dgVector dir1 (n * normal);
                        dgFloat32 projection = dir0 % dir1;
                        if (projection <= dgFloat32 (0.0f)) {
                            normal = n;
                        }
                        normal = polygonInstance->m_globalMatrix.RotateVector(normal);
                        for (dgInt32 i = 0; i < count; i ++) {
                            contactsOut[i].m_normal = normal;
                            //contactsOut[i].m_userId = m_faceId;
                            contactsOut[i].m_shapeId0 = hullId;
                            contactsOut[i].m_shapeId1 = m_faceId;
                        }
                    } else {
                        for (dgInt32 i = 0; i < count; i ++) {
                            //contactsOut[i].m_userId = m_faceId;
                            contactsOut[i].m_shapeId0 = hullId;
                            contactsOut[i].m_shapeId1 = m_faceId;
                        }
                    }
                }
#endif

                for (dgInt32 i = 0; i < count; i ++) {
                    contactsOut[i].m_shapeId0 = hullId;
                    contactsOut[i].m_shapeId1 = m_faceId;
                }
            }
        }
    }

    proxy.m_matrix = savedProxyMatrix;
    return count;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy)
{
	dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI));
	dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));

	dgAssert(this == proxy.m_instance1->GetChildShape());
	dgAssert(m_count);
	dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));

	const dgBody* const body0 = proxy.m_body0;
	const dgBody* const body1 = proxy.m_body1;

	dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity());

	dgVector relativeVelocity (body0->m_veloc - body1->m_veloc);
	if (m_normal.DotProduct4(relativeVelocity).GetScalar() >= 0.0f) {
		return 0;
	}
	dgFloat32 den = dgFloat32 (1.0f) / (relativeVelocity % m_normal);
	if (den > dgFloat32 (1.0e-5f)) {
		// this can actually happens
		dgAssert(0);
		return 0;
	}

	dgContact* const contactJoint = proxy.m_contactJoint;
	contactJoint->m_closestDistance = dgFloat32(1.0e10f);

	dgMatrix polygonMatrix;
	dgVector right (m_localPoly[1] - m_localPoly[0]);
	polygonMatrix[0] = right.CompProduct4(right.InvMagSqrt());
	polygonMatrix[1] = m_normal;
	polygonMatrix[2] = polygonMatrix[0] * m_normal;
	polygonMatrix[3] = dgVector::m_wOne;
	dgAssert (polygonMatrix.TestOrthogonal());

	dgVector polyBoxP0(dgFloat32(1.0e15f));
	dgVector polyBoxP1(dgFloat32(-1.0e15f));
	for (dgInt32 i = 0; i < m_count; i++) {
		dgVector point (polygonMatrix.UnrotateVector(m_localPoly[i]));
		polyBoxP0 = polyBoxP0.GetMin(point);
		polyBoxP1 = polyBoxP1.GetMax(point);
	}

	dgVector hullBoxP0;
	dgVector hullBoxP1;
	dgMatrix hullMatrix (polygonMatrix * proxy.m_instance0->m_globalMatrix);
	proxy.m_instance0->CalcAABB(hullMatrix, hullBoxP0, hullBoxP1);
	dgVector minBox(polyBoxP0 - hullBoxP1);
	dgVector maxBox(polyBoxP1 - hullBoxP0);
	dgVector veloc (polygonMatrix.UnrotateVector (relativeVelocity));
	dgFastRayTest ray(dgVector(dgFloat32(0.0f)), veloc);
 	dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);

	dgInt32 count = 0;
	if (distance < dgFloat32(1.0f)) {
		bool inside = false;

		dgVector boxSize((hullBoxP1 - hullBoxP0).CompProduct4(dgVector::m_half));
		dgVector sphereMag2 (boxSize.DotProduct4(boxSize));
		boxSize = sphereMag2.Sqrt();

		dgVector pointInPlane (polygonMatrix.RotateVector(hullBoxP1 + hullBoxP0).CompProduct4(dgVector::m_half));
		dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;

		dgFloat32 timeToPlane0 = (distToPlane + boxSize.GetScalar()) * den;
		dgFloat32 timeToPlane1 = (distToPlane - boxSize.GetScalar()) * den;

		dgVector boxOrigin0 (pointInPlane + relativeVelocity.Scale4(timeToPlane0));
		dgVector boxOrigin1 (pointInPlane + relativeVelocity.Scale4(timeToPlane1));
		dgVector boxOrigin ((boxOrigin0 + boxOrigin1).CompProduct4(dgVector::m_half)); 
		dgVector boxProjectSize (((boxOrigin0 - boxOrigin1).CompProduct4(dgVector::m_half))); 
		sphereMag2 = boxProjectSize.DotProduct4(boxProjectSize);
		boxSize = sphereMag2.Sqrt();

		dgAssert (boxOrigin.m_w == 0.0f);
		boxOrigin = boxOrigin | dgVector::m_wOne;
		
		if (!proxy.m_intersectionTestOnly) {
			inside = true;
			dgInt32 i0 = m_count - 1;

			for (dgInt32 i = 0; i < m_count; i++) {
				dgVector e(m_localPoly[i] - m_localPoly[i0]);
				dgVector n(m_normal * e & dgVector::m_triplexMask);
				dgFloat32 param = dgSqrt (sphereMag2.GetScalar() / (n.DotProduct4(n)).GetScalar());
				dgPlane plane(n, -(m_localPoly[i0] % n));

				dgVector p0 (boxOrigin + n.Scale4 (param));
				dgVector p1 (boxOrigin - n.Scale4 (param));

				dgFloat32 size0 = (plane.DotProduct4 (p0)).GetScalar();
				dgFloat32 size1 = (plane.DotProduct4 (p1)).GetScalar();

				if ((size0 < 0.0f) && (size1 < 0.0f)) {
					return 0;
				}

				if ((size0 * size1) < 0.0f) {
					inside = false;
					break;
				}
				i0 = i;
			}
		}

		dgFloat32 convexSphapeUmbra = dgMax (proxy.m_instance0->GetUmbraClipSize(), boxSize.GetScalar());
		if (m_faceClipSize > convexSphapeUmbra) {
			BeamClipping(boxOrigin, convexSphapeUmbra);
			m_faceClipSize = proxy.m_instance0->m_childShape->GetBoxMaxRadius();
		}

		const dgInt32 hullId = proxy.m_instance0->GetUserDataID();
		if (inside & !proxy.m_intersectionTestOnly) {
			const dgMatrix& matrixInstance0 = proxy.m_instance0->m_globalMatrix;
			dgVector normalInHull(matrixInstance0.UnrotateVector(m_normal.Scale4(dgFloat32(-1.0f))));
			dgVector pointInHull(proxy.m_instance0->SupportVertex(normalInHull, NULL));
			dgVector p0 (matrixInstance0.TransformVector(pointInHull));

			dgFloat32 timetoImpact = dgFloat32(0.0f);
			dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
			if (penetration < dgFloat32(0.0f)) {
				timetoImpact = penetration / (relativeVelocity % m_normal);
				dgAssert(timetoImpact >= dgFloat32(0.0f));
			}

			if (timetoImpact <= proxy.m_timestep) {
				dgVector contactPoints[64];
				contactJoint->m_closestDistance = penetration;
				proxy.m_timestep = timetoImpact;
				proxy.m_normal = m_normal;
				proxy.m_closestPointBody0 = p0;
				proxy.m_closestPointBody1 = p0 + m_normal.Scale4(penetration);

				if (!proxy.m_intersectionTestOnly) {
					pointInHull -= normalInHull.Scale4 (DG_ROBUST_PLANE_CLIP);
					count = proxy.m_instance0->CalculatePlaneIntersection(normalInHull, pointInHull, contactPoints);

					dgVector step(relativeVelocity.Scale4(timetoImpact));
					penetration = dgMax(penetration, dgFloat32(0.0f));
					dgContactPoint* const contactsOut = proxy.m_contacts;
					for (dgInt32 i = 0; i < count; i++) {
						contactsOut[i].m_point = matrixInstance0.TransformVector(contactPoints[i]) + step;
						contactsOut[i].m_normal = m_normal;
						contactsOut[i].m_shapeId0 = hullId;
						contactsOut[i].m_shapeId1 = m_faceId;
						contactsOut[i].m_penetration = penetration;
					}
				}
			}
		} else {
			m_vertexCount = dgUnsigned16 (m_count);
			count = world->CalculateConvexToConvexContacts(proxy);
			if (count >= 1) {
				dgContactPoint* const contactsOut = proxy.m_contacts;
				for (dgInt32 i = 0; i < count; i++) {
					contactsOut[i].m_shapeId0 = hullId;
					contactsOut[i].m_shapeId1 = m_faceId;
				}
			}
		}
	}

	return count;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy)
{
	dgInt32 count = 0;

	dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI));
	dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
	dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity());

	const dgCollisionInstance* const polygonInstance = proxy.m_instance1;
	dgAssert(this == polygonInstance->GetChildShape());
	dgAssert(m_count);
	dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));

	const dgMatrix& hullMatrix = proxy.m_instance0->m_globalMatrix;
	dgContact* const contactJoint = proxy.m_contactJoint;
	const dgCollisionInstance* const hull = proxy.m_instance0;

	dgVector normalInHull(hullMatrix.UnrotateVector(m_normal));
	dgVector pointInHull(hull->SupportVertex(normalInHull.Scale4(dgFloat32(-1.0f)), NULL));
	dgVector p0(hullMatrix.TransformVector(pointInHull));

	dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
	if (penetration < dgFloat32(0.0f)) {
		return 0;
	}

	dgVector p1(hullMatrix.TransformVector(hull->SupportVertex(normalInHull, NULL)));
	contactJoint->m_closestDistance = dgFloat32(0.0f);
	dgFloat32 distance = (m_localPoly[0] - p1) % m_normal;
	if (distance >= dgFloat32(0.0f)) {
		return 0;
	}

	dgVector boxSize (hull->GetBoxSize() & dgVector::m_triplexMask);
	dgVector boxOrigin ((hull->GetBoxOrigin() & dgVector::m_triplexMask) + dgVector::m_wOne);

	bool inside = true;
	dgInt32 i0 = m_count - 1;
	for (dgInt32 i = 0; i < m_count; i++) {
		dgVector e(m_localPoly[i] - m_localPoly[i0]);
		dgVector edgeBoundaryNormal(m_normal * e);
		dgPlane plane(edgeBoundaryNormal, - m_localPoly[i0].DotProduct4 (edgeBoundaryNormal).GetScalar());
		plane = hullMatrix.TransformPlane(plane);

		dgFloat32 supportDist = boxSize.DotProduct4 (plane.Abs()).GetScalar();
		dgFloat32 centerDist = plane.DotProduct4 (boxOrigin).GetScalar();

		if ((centerDist + supportDist) < dgFloat32(0.0f)) {
			return 0;
		}

		if ((centerDist - supportDist) < dgFloat32(0.0f)) {
			inside = false;
			break;
		}
		i0 = i;
	}

//inside = false;
	dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize();
	if (m_faceClipSize > convexSphapeUmbra) {
		BeamClipping(dgVector(dgFloat32(0.0f)), convexSphapeUmbra);
		m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
	}

	const dgInt32 hullId = hull->GetUserDataID();
	if (inside & !proxy.m_intersectionTestOnly) {
		dgAssert(penetration >= dgFloat32(0.0f));
		dgVector contactPoints[64];

		dgAssert(penetration >= 0.0f);
		dgVector point(pointInHull + normalInHull.Scale4(penetration + DG_ROBUST_PLANE_CLIP));

		count = hull->CalculatePlaneIntersection(normalInHull.Scale4(dgFloat32(-1.0f)), point, contactPoints);
		dgVector step(normalInHull.Scale4((proxy.m_skinThickness - penetration) * dgFloat32(0.5f)));

		dgContactPoint* const contactsOut = proxy.m_contacts;
		dgAssert(contactsOut);
		for (dgInt32 i = 0; i < count; i++) {
			contactsOut[i].m_point = hullMatrix.TransformVector(contactPoints[i] + step);
			contactsOut[i].m_normal = m_normal;
			contactsOut[i].m_shapeId0 = hullId;
			contactsOut[i].m_shapeId1 = m_faceId;
			contactsOut[i].m_penetration = penetration;
		}
	} else {
		m_vertexCount = dgUnsigned16 (m_count);
		count = world->CalculateConvexToConvexContacts(proxy);
		dgAssert(proxy.m_intersectionTestOnly || (count >= 0));

		if (count >= 1) {
			dgContactPoint* const contactsOut = proxy.m_contacts;
			if (m_closestFeatureType == 3) {
				for (dgInt32 i = 0; i < count; i++) {
					//contactsOut[i].m_userId = m_faceId;
					contactsOut[i].m_shapeId0 = hullId;
					contactsOut[i].m_shapeId1 = m_faceId;
				}
			} else {
				dgVector normal (contactsOut[0].m_normal);

				if (normal.DotProduct4(m_normal).GetScalar() < dgFloat32(0.9995f)) {
					dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
					dgVector adjacentNormal (CalculateGlobalNormal (parentMesh, dgVector(&m_vertex[index * m_stride])));
					if ((m_normal.DotProduct4(adjacentNormal).GetScalar() > dgFloat32(0.9995f))) {
						normal = adjacentNormal;
					} else {
						dgVector dir0(adjacentNormal * m_normal);
						dgVector dir1(adjacentNormal * normal);
						dgFloat32 projection = dir0.DotProduct4(dir1).GetScalar();
						if (projection <= dgFloat32(0.0f)) {
							normal = adjacentNormal;
						}
					}
					normal = polygonInstance->m_globalMatrix.RotateVector(normal);

					for (dgInt32 i = 0; i < count; i++) {
						contactsOut[i].m_normal = normal;
						contactsOut[i].m_shapeId0 = hullId;
						contactsOut[i].m_shapeId1 = m_faceId;
					}
				} else {
					for (dgInt32 i = 0; i < count; i++) {
						contactsOut[i].m_shapeId0 = hullId;
						contactsOut[i].m_shapeId1 = m_faceId;
					}
				}
			}
		}
	}
	return count;
}
void setupPhysics(hkpWorld* physicsWorld)
{
    //
    //  Create the ground box
    //
    {
        hkVector4 groundRadii( 70.0f, 2.0f, 140.0f );
        hkpConvexShape* shape = new hkpBoxShape( groundRadii , 0 );

        hkpRigidBodyCinfo ci;

        ci.m_shape = shape;
        ci.m_motionType = hkpMotion::MOTION_FIXED;
        ci.m_position = hkVector4( 0.0f, -2.0f, 0.0f );
        ci.m_qualityType = HK_COLLIDABLE_QUALITY_FIXED;

        physicsWorld->addEntity( new hkpRigidBody( ci ) )->removeReference();
        shape->removeReference();
    }

    hkVector4 groundPos( 0.0f, 0.0f, 0.0f );
    hkVector4 posy = groundPos;

    //
    // Create the walls
    //

    int wallHeight = 8;
    int wallWidth  = 8;
    int numWalls = 6;
    hkVector4 boxSize( 1.0f, 0.5f, 0.5f);
    hkpBoxShape* box = new hkpBoxShape( boxSize , 0 );
    box->setRadius( 0.0f );

    hkReal deltaZ = 25.0f;
    posy(2) = -deltaZ * numWalls * 0.5f;

    for ( int y = 0; y < numWalls; y ++ )			// first wall
    {
        createBrickWall( physicsWorld, wallHeight, wallWidth, posy, 0.2f, box, boxSize );
        posy(2) += deltaZ;
    }
    box->removeReference();

    //
    // Create a ball moving towards the walls
    //

    const hkReal radius = 1.5f;
    const hkReal sphereMass = 150.0f;

    hkVector4 relPos( 0.0f,radius + 0.0f, 50.0f );

    hkpRigidBodyCinfo info;
    hkpMassProperties massProperties;
    hkpInertiaTensorComputer::computeSphereVolumeMassProperties(radius, sphereMass, massProperties);

    info.m_mass = massProperties.m_mass;
    info.m_centerOfMass  = massProperties.m_centerOfMass;
    info.m_inertiaTensor = massProperties.m_inertiaTensor;
    info.m_shape = new hkpSphereShape( radius );
    info.m_position.setAdd4(posy, relPos );
    info.m_motionType  = hkpMotion::MOTION_BOX_INERTIA;

    info.m_qualityType = HK_COLLIDABLE_QUALITY_BULLET;


    hkpRigidBody* sphereRigidBody = new hkpRigidBody( info );
    g_ball = sphereRigidBody;

    physicsWorld->addEntity( sphereRigidBody );
    sphereRigidBody->removeReference();
    info.m_shape->removeReference();

    hkVector4 vel(  0.0f,4.9f, -100.0f );
    sphereRigidBody->setLinearVelocity( vel );


}
UserCollisionFilterDemo::UserCollisionFilterDemo( hkDemoEnvironment* env)
	: hkDefaultPhysicsDemo(env)
{
	//
	// Setup the camera
	//
	{
		hkVector4 from(10.0f, 10.0f, 15.0f);
		hkVector4 to(0.0f, 3.0f, 0.0f);
		hkVector4 up(0.0f, 1.0f, 0.0f);
		setupDefaultCameras( env, from, to, up );
	}


	//
	// Create the world
	//
	{
		hkpWorldCinfo info;
		m_world = new hkpWorld( info );
		m_world->lock();

		setupGraphics();
	}

	//
	// Register the box-box collision agent
	//
	{
		hkpAgentRegisterUtil::registerAllAgents( m_world->getCollisionDispatcher() );
	}

	/// To add the filter to the world we simply instantiate a new filter and set it to be active:

	//
	//	Create the my collision filter
	//
	{
		MyCollisionFilter* filter = new MyCollisionFilter();
	
		m_world->setCollisionFilter( filter );
		filter->removeReference();
	}

	// NOTE: You must set the collision filter prior to adding any entities to the world otherwise the pre-filter
	// added entities could cause undesirable behaviour before the filter is active.

	// All three objects are hkpBoxShape shapes, two of which are fixed and the third dynamic. In order to allow
	// our filters work correctly we must assign each of the objects a collision 'group'. Both the lower floor
	// and the dynamic box have their collision 'group' set to zero: 

	//
	// Create the floor with collisioninfo = 0
	//
	{
		hkpRigidBodyCinfo info;
		hkVector4 fixedBoxSize(5.0f, .5f , 5.0f );
		hkpBoxShape* fixedBoxShape = new hkpBoxShape( fixedBoxSize , 0 );
		info.m_shape = fixedBoxShape;
		info.m_motionType = hkpMotion::MOTION_FIXED;
		info.m_position.set(0.0f, -1.0f, 0.0f);

		// Create fixed box
		hkpRigidBody* floor = new hkpRigidBody(info);
		floor->setCollisionFilterInfo(0);

		m_world->addEntity(floor);
		floor->removeReference();
		fixedBoxShape->removeReference();
	}

	// Whereas the upper floor belongs to the 'one' group:

	//
	// Create the floor with collisioninfo = 1
	//
	{
		hkpRigidBodyCinfo info;
		hkVector4 fixedBoxSize(3.0f, .5f , 3.0f );
		hkpBoxShape* fixedBoxShape = new hkpBoxShape( fixedBoxSize , 0 );
		info.m_shape = fixedBoxShape;
		info.m_motionType = hkpMotion::MOTION_FIXED;
		info.m_position.set(0.0f,4.0f,0.0f);

		// Create fixed box
		hkpRigidBody* floor = new hkpRigidBody(info);
		fixedBoxShape->removeReference();

		floor->setCollisionFilterInfo(1);

		m_world->addEntity(floor);
		floor->removeReference();
	}

	// In this way only the lower floor and the dynamic box will interact based on the rule we defined in
	// MyCollisionFilter::isCollisionEnabled(...).

	//
	// Create a moving box with collisioninfo = 0, so it will collide with only the "bottom" floor
	//
	{
		hkpRigidBodyCinfo boxInfo;
		hkVector4 boxSize( .5f, .5f ,.5f );
		hkpShape* boxShape = new hkpBoxShape( boxSize , 0 );
		boxInfo.m_shape = boxShape;
		boxInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA;

		// Compute the box inertia tensor
		hkReal boxMass = 1.0f;
		hkpMassProperties massProperties;
		hkpInertiaTensorComputer::computeBoxVolumeMassProperties( boxSize, boxMass, massProperties );
		boxInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA;

		boxInfo.m_mass = boxMass;
		boxInfo.m_inertiaTensor = massProperties.m_inertiaTensor;
		boxInfo.m_position.set(0.0f, 7.0f, 0.0f);

		hkpRigidBody* boxRigidBody = new hkpRigidBody( boxInfo );
		boxShape->removeReference();

		boxRigidBody->setCollisionFilterInfo(0);	// HERE WE SET THE COLLISION FILTER INFO
		m_world->addEntity( boxRigidBody );
		boxRigidBody->removeReference();
	}

	m_world->unlock();
}
dgInt32 dgConvexHull4d::InitVertexArray(dgHullVector* const points, const dgBigVector* const vertexCloud, dgInt32 count, void* const memoryPool, dgInt32 maxMemSize)
{
	for (dgInt32 i = 0; i < count; i ++) {
		points[i] = vertexCloud[i];
		points[i].m_index = i;
		points[i].m_mark = 0;
	}

	dgSort(points, count, ConvexCompareVertex);
	dgInt32 indexCount = 0;
	for (int i = 1; i < count; i ++) {
		for (; i < count; i ++) {
			if (ConvexCompareVertex (&points[indexCount], &points[i], NULL)) {
				indexCount ++;
				points[indexCount] = points[i];
				break;
			}
		}
	}
	count = indexCount + 1;
	if (count < 4) {
		m_count = 0;
		return count;
	}

	dgAABBPointTree4d* tree = BuildTree (NULL, points, count, 0, (dgInt8**) &memoryPool, maxMemSize);

	dgBigVector boxSize (tree->m_box[1] - tree->m_box[0]);	
	boxSize.m_w = dgFloat64 (0.0f);
	m_diag = dgFloat32 (sqrt (boxSize.DotProduct4(boxSize).m_x));

	m_points[4].m_x = dgFloat64 (0.0f);
	dgHullVector* const convexPoints = &m_points[0]; 
	dgStack<dgBigVector> normalArrayPool (256);
	dgBigVector* const normalArray = &normalArrayPool[0];
	dgInt32 normalCount = BuildNormalList (&normalArray[0]);

	dgInt32 index = SupportVertex (&tree, points, normalArray[0]);
	convexPoints[0] = points[index];
	points[index].m_mark = 1;

	bool validTetrahedrum = false;
	dgBigVector e1 (dgFloat64 (0.0f), dgFloat64 (0.0f), dgFloat64 (0.0f), dgFloat64 (0.0f)) ;
	for (dgInt32 i = 1; i < normalCount; i ++) {
		dgInt32 index = SupportVertex (&tree, points, normalArray[i]);
		dgAssert (index >= 0);
		e1 = points[index] - convexPoints[0];
		e1.m_w = dgFloat64 (0.0f);
		dgFloat64 error2 = e1.DotProduct4(e1).m_x;
		if (error2 > (dgFloat32 (1.0e-4f) * m_diag * m_diag)) {
			convexPoints[1] = points[index];
			points[index].m_mark = 1;
			validTetrahedrum = true;
			break;
		}
	}
	if (!validTetrahedrum) {
		m_count = 0;
		return count;
	}


	dgInt32 bestIndex = -1;
	dgFloat64 bestValue = dgFloat64 (1.0f);
	validTetrahedrum = false;
	dgFloat64 lenght2 = e1.DotProduct4(e1).m_x;
	dgBigVector e2(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));;
	for (dgInt32 i = 2; i < normalCount; i ++) {
		dgInt32 index = SupportVertex (&tree, points, normalArray[i]);
		dgAssert (index >= 0);
		dgAssert (index < count);
		e2 = points[index] - convexPoints[0];
		e2.m_w = dgFloat64 (0.0f);
		dgFloat64 den = e2.DotProduct4(e2).m_x;
		if (fabs (den) > (dgFloat64 (1.0e-6f) * m_diag)) {
			den = sqrt (lenght2 * den);
			dgFloat64 num = e2.DotProduct4(e1).m_x;
			dgFloat64 cosAngle = fabs (num / den);
			if (cosAngle < bestValue) {
				bestValue = cosAngle;
				bestIndex = index;
			}

			if (cosAngle < 0.9f) {
				break;
			}
		}
	}

	if (bestValue < dgFloat64 (0.999f)) {
		convexPoints[2] = points[bestIndex];
		points[bestIndex].m_mark = 1;
		validTetrahedrum = true;
	}

	if (!validTetrahedrum) {
		m_count = 0;
		return count;
	}

	validTetrahedrum = false;
	dgBigVector e3(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));;
	for (dgInt32 i = 3; i < normalCount; i ++) {
		dgInt32 index = SupportVertex (&tree, points, normalArray[i]);
		dgAssert (index >= 0);
		dgAssert (index < count);

		e3 = points[index] - convexPoints[0];
		e3.m_w = dgFloat64 (0.0f);
		dgFloat64 volume = (e1 * e2) % e3;		
		if (fabs (volume) > (dgFloat64 (1.0e-4f) * m_diag * m_diag * m_diag)) {
			convexPoints[3] = points[index];
			points[index].m_mark = 1;
			validTetrahedrum = true;
			break;
		}
	}

	m_count = 4;
	if (!validTetrahedrum) {
		m_count = 0;
	}
	
	return count;
}
void Restitution (DemoEntityManager* const scene)
{
	scene->CreateSkyBox();

	// customize the scene after loading
	// set a user friction variable in the body for variable friction demos
	// later this will be done using LUA script
	NewtonWorld* const world = scene->GetNewton();
	dMatrix offsetMatrix (dGetIdentityMatrix());

	int defaultMaterialID = NewtonMaterialGetDefaultGroupID (world);
	NewtonMaterialSetCollisionCallback (world, defaultMaterialID, defaultMaterialID, NULL, UserContactRestitution); 


	CreateLevelMesh (scene, "flatPlane.ngd", 0);

	dVector location (0.0f, 0.0f, 0.0f, 0.0f);
	dVector size (0.5f, 0.5f, 1.0f, 0.0f);

	// create some spheres 
	dVector sphSize (1.0f, 1.0f, 1.0f, 0.0f);
	NewtonCollision* const sphereCollision = CreateConvexCollision (world, offsetMatrix, sphSize, _SPHERE_PRIMITIVE, 0);
	DemoMesh* const sphereMesh = new DemoMesh("sphere", sphereCollision, "smilli.tga", "smilli.tga", "smilli.tga");

	// create some boxes too
	dVector boxSize (1.0f, 0.5f, 2.0f, 0.0f);
	NewtonCollision* const boxCollision = CreateConvexCollision (world, offsetMatrix, boxSize, _BOX_PRIMITIVE, 0);
	DemoMesh* const boxMesh = new DemoMesh("box", boxCollision, "smilli.tga", "smilli.tga", "smilli.tga");


	int zCount = 10;
	dFloat spacing = 4.0f;
	dMatrix matrix (dGetIdentityMatrix());
	dVector origin (matrix.m_posit);
	origin.m_x -= 0.0f;

	// create a simple scene
	for (int i = 0; i < zCount; i ++) {
		dFloat z;
		dFloat x;
		dFloat mass;
		dVector size (1.0f, 0.5f, 2.0f, 0.0f);

		x = origin.m_x;
		z = origin.m_z + (i - zCount / 2) * spacing;

		mass = 1.0f;
		matrix.m_posit = FindFloor (world, dVector (x, 100.0f, z), 200.0f);
		matrix.m_posit.m_w = 1.0f;
		
		float restitution;
		NewtonBody* body;
		NewtonCollision* collision;
		

		matrix.m_posit.m_y += 4.0f;
		body = CreateSimpleSolid (scene, sphereMesh, mass, matrix, sphereCollision, defaultMaterialID);
		NewtonBodySetLinearDamping (body, 0.0f);
		collision = NewtonBodyGetCollision(body);
		restitution = i * 0.1f + 0.083f;
		NewtonCollisionSetUserData (collision, *((void**)&restitution));

		matrix.m_posit.m_y += 4.0f;
		//body = CreateSimpleSolid (scene, sphereMesh, mass, matrix, sphereCollision, defaultMaterialID, shapeOffsetMatrix);
		body = CreateSimpleSolid (scene, boxMesh, mass, matrix, boxCollision, defaultMaterialID);
		NewtonBodySetLinearDamping (body, 0.0f);
		collision = NewtonBodyGetCollision(body);
		restitution = i * 0.1f + 0.083f;
		NewtonCollisionSetUserData (collision, *((void**)&restitution));

		matrix.m_posit.m_y += 4.0f;
		body = CreateSimpleSolid (scene, sphereMesh, mass, matrix, sphereCollision, defaultMaterialID);
		NewtonBodySetLinearDamping (body, 0.0f);
		collision = NewtonBodyGetCollision(body);
		restitution = i * 0.1f + 0.083f;
		NewtonCollisionSetUserData (collision, *((void**)&restitution));

		matrix.m_posit.m_y += 4.0f;
		dVector boxSize (1.0f, 0.5f, 2.0f, 0.0f);
		body = CreateSimpleSolid (scene, boxMesh, mass, matrix, boxCollision, defaultMaterialID);
		NewtonBodySetLinearDamping (body, 0.0f);
		collision = NewtonBodyGetCollision(body);
		restitution = i * 0.1f + 0.083f;
		NewtonCollisionSetUserData (collision, *((void**)&restitution));
	}

	boxMesh->Release();
	sphereMesh->Release();
	NewtonDestroyCollision(boxCollision);
	NewtonDestroyCollision(sphereCollision);


	dMatrix camMatrix (dGetIdentityMatrix());
	dQuaternion rot (camMatrix);
	origin = dVector (-25.0f, 5.0f, 0.0f, 0.0f);
	scene->SetCameraMatrix(rot, origin);

}
Beispiel #27
0
std::unique_ptr<Shape> Shape::createShape(const BasicShape* basicShape, const LayoutSize& logicalBoxSize, WritingMode writingMode, float margin)
{
    ASSERT(basicShape);

    bool horizontalWritingMode = isHorizontalWritingMode(writingMode);
    float boxWidth = horizontalWritingMode ? logicalBoxSize.width() : logicalBoxSize.height();
    float boxHeight = horizontalWritingMode ? logicalBoxSize.height() : logicalBoxSize.width();
    std::unique_ptr<Shape> shape;

    switch (basicShape->type()) {

    case BasicShape::BasicShapeCircleType: {
        const BasicShapeCircle* circle = static_cast<const BasicShapeCircle*>(basicShape);
        float centerX = floatValueForCenterCoordinate(circle->centerX(), boxWidth);
        float centerY = floatValueForCenterCoordinate(circle->centerY(), boxHeight);
        float radius = circle->floatValueForRadiusInBox(boxWidth, boxHeight);
        FloatPoint logicalCenter = physicalPointToLogical(FloatPoint(centerX, centerY), logicalBoxSize.height(), writingMode);

        shape = createCircleShape(logicalCenter, radius);
        break;
    }

    case BasicShape::BasicShapeEllipseType: {
        const BasicShapeEllipse* ellipse = static_cast<const BasicShapeEllipse*>(basicShape);
        float centerX = floatValueForCenterCoordinate(ellipse->centerX(), boxWidth);
        float centerY = floatValueForCenterCoordinate(ellipse->centerY(), boxHeight);
        float radiusX = ellipse->floatValueForRadiusInBox(ellipse->radiusX(), centerX, boxWidth);
        float radiusY = ellipse->floatValueForRadiusInBox(ellipse->radiusY(), centerY, boxHeight);
        FloatPoint logicalCenter = physicalPointToLogical(FloatPoint(centerX, centerY), logicalBoxSize.height(), writingMode);

        shape = createEllipseShape(logicalCenter, FloatSize(radiusX, radiusY));
        break;
    }

    case BasicShape::BasicShapePolygonType: {
        const BasicShapePolygon& polygon = *static_cast<const BasicShapePolygon*>(basicShape);
        const Vector<Length>& values = polygon.values();
        size_t valuesSize = values.size();
        ASSERT(!(valuesSize % 2));
        std::unique_ptr<Vector<FloatPoint>> vertices = std::make_unique<Vector<FloatPoint>>(valuesSize / 2);
        for (unsigned i = 0; i < valuesSize; i += 2) {
            FloatPoint vertex(
                floatValueForLength(values.at(i), boxWidth),
                floatValueForLength(values.at(i + 1), boxHeight));
            (*vertices)[i / 2] = physicalPointToLogical(vertex, logicalBoxSize.height(), writingMode);
        }

        shape = createPolygonShape(WTF::move(vertices), polygon.windRule());
        break;
    }

    case BasicShape::BasicShapeInsetType: {
        const BasicShapeInset& inset = *static_cast<const BasicShapeInset*>(basicShape);
        float left = floatValueForLength(inset.left(), boxWidth);
        float top = floatValueForLength(inset.top(), boxHeight);
        FloatRect rect(left,
            top,
            std::max<float>(boxWidth - left - floatValueForLength(inset.right(), boxWidth), 0),
            std::max<float>(boxHeight - top - floatValueForLength(inset.bottom(), boxHeight), 0));
        FloatRect logicalRect = physicalRectToLogical(rect, logicalBoxSize.height(), writingMode);

        FloatSize boxSize(boxWidth, boxHeight);
        FloatSize topLeftRadius = physicalSizeToLogical(floatSizeForLengthSize(inset.topLeftRadius(), boxSize), writingMode);
        FloatSize topRightRadius = physicalSizeToLogical(floatSizeForLengthSize(inset.topRightRadius(), boxSize), writingMode);
        FloatSize bottomLeftRadius = physicalSizeToLogical(floatSizeForLengthSize(inset.bottomLeftRadius(), boxSize), writingMode);
        FloatSize bottomRightRadius = physicalSizeToLogical(floatSizeForLengthSize(inset.bottomRightRadius(), boxSize), writingMode);
        FloatRoundedRect::Radii cornerRadii(topLeftRadius, topRightRadius, bottomLeftRadius, bottomRightRadius);

        cornerRadii.scale(calcBorderRadiiConstraintScaleFor(logicalRect, cornerRadii));

        shape = createInsetShape(FloatRoundedRect(logicalRect, cornerRadii));
        break;
    }

    default:
        ASSERT_NOT_REACHED();
    }

    shape->m_writingMode = writingMode;
    shape->m_margin = margin;

    return shape;
}
SlidingWorldDemo::SlidingWorldDemo(hkDemoEnvironment* env)
:   hkDefaultPhysicsDemo(env), m_time(0.0f), m_ticks(1), m_currentMode(MANUAL_SHIFT), m_delayBetweenAutomaticShifts(90)
{
	// Disable warnings:									
	hkError::getInstance().setEnabled(0xf0ff00a1, false); //'Attempting to remove an entity twice)'


	// Set up the camera.
	{
		hkVector4 from(0.0f, 75.0f, 30.0f);
		hkVector4 to(0.0f, 3.0f, 0.0f);
		hkVector4 up(0.0f, 1.0f, 0.0f);
		setupDefaultCameras( env, from, to, up );
	}

	m_centers[0].set(10, 0, 0);
	m_centers[1].set(10, 0, 10);
	m_centers[2].set(0, 0, 10);
	m_centers[3].set(-10, 0, 10);
	m_centers[4].set(-10, 0, 0);
	m_centers[5].set(-10, 0, -10);
	m_centers[6].set(0, 0, -10);
	m_centers[7].set(10, 0, -10);

	m_currentCenter.setAll(0);


	// Create the world.
	{
		hkpWorldCinfo info;
		info.setupSolverInfo(hkpWorldCinfo::SOLVER_TYPE_4ITERS_MEDIUM);

		//info.setBroadPhaseWorldSize( 120.0f );
		info.setBroadPhaseWorldSize( 30.0f );

		m_world = new hkpWorld( info );
		m_world->m_wantDeactivation = true;
		m_world->lock();

		// N.B. We need this border 'for safety': it should play no part in the 'broadphase moving' or the 'coordinate shfting' -
		// the border callbacks are *not* fired as a result of calls to hkBroadphase::shiftBroadPhase() or hkBroadphase::shiftAllObjects().
		// They will be fired as a result of body addition or world stepping, so this will ensure that bodies added or moved 
		// (under integration) get correctly removed from simulation so that the only body not fully contained inside the broadphase
		// is a single fixed body (assumed to be a large, level-encompassing landscape tile.
		SlidingWorldBroadphaseBorder *border = new SlidingWorldBroadphaseBorder( m_world );
		m_world->setBroadPhaseBorder(border);
		border->removeReference();

		// Setup the rest of the default viewers:
		setupGraphics();

		// Register all collision agents
		hkpAgentRegisterUtil::registerAllAgents( m_world->getCollisionDispatcher() );
	}


	// Create the fixed floor box.
	{
		hkVector4 fixedBoxSize(30.0f, .5f , 30.0f );

		hkpRigidBodyCinfo info;
		info.m_shape = new hkpBoxShape( fixedBoxSize , 0 );
		info.m_motionType = hkpMotion::MOTION_FIXED;

		// Create fixed box.
		hkpRigidBody* box = new hkpRigidBody(info);
		m_world->addEntity(box)->removeReference();

		info.m_shape->removeReference();
	}

	{
		// Create a single shape, and reuse it for all bodies.
		hkVector4 boxSize( .75f, .75f ,.75f );
		hkpShape* boxShape = new hkpBoxShape( boxSize , 0 );

		hkpRigidBodyCinfo boxInfo;
		boxInfo.m_shape = boxShape;
		boxInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA;

		// Compute the box inertia tensor.
		hkpInertiaTensorComputer::setShapeVolumeMassProperties(boxInfo.m_shape, 5.0f, boxInfo);

		boxInfo.m_rigidBodyDeactivatorType = hkpRigidBodyDeactivator::DEACTIVATOR_NEVER;

		for (int i = -20; i <= 20; i += 5)
		{
			for (int j = -20; j <= 20; j += 5)
			{
				boxInfo.m_position.set( (hkReal)i, 2.0f, (hkReal)j );

				hkpRigidBody* boxRigidBody = new hkpRigidBody(boxInfo);
				m_world->addEntity( boxRigidBody);
				m_boxes.pushBack(boxRigidBody);
			}
		}

		boxShape->removeReference();
				
	}

	m_world->unlock();
}
Beispiel #29
0
PassOwnPtr<Shape> Shape::createShape(const BasicShape* basicShape, const LayoutSize& logicalBoxSize, WritingMode writingMode, float margin)
{
    ASSERT(basicShape);

    bool horizontalWritingMode = isHorizontalWritingMode(writingMode);
    float boxWidth = horizontalWritingMode ? logicalBoxSize.width().toFloat() : logicalBoxSize.height().toFloat();
    float boxHeight = horizontalWritingMode ? logicalBoxSize.height().toFloat() : logicalBoxSize.width().toFloat();
    OwnPtr<Shape> shape;

    switch (basicShape->type()) {

    case BasicShape::BasicShapeCircleType: {
        const BasicShapeCircle* circle = toBasicShapeCircle(basicShape);
        FloatPoint center = floatPointForCenterCoordinate(circle->centerX(), circle->centerY(), FloatSize(boxWidth, boxHeight));
        float radius = circle->floatValueForRadiusInBox(FloatSize(boxWidth, boxHeight));
        FloatPoint logicalCenter = physicalPointToLogical(center, logicalBoxSize.height().toFloat(), writingMode);

        shape = createCircleShape(logicalCenter, radius);
        break;
    }

    case BasicShape::BasicShapeEllipseType: {
        const BasicShapeEllipse* ellipse = toBasicShapeEllipse(basicShape);
        FloatPoint center = floatPointForCenterCoordinate(ellipse->centerX(), ellipse->centerY(), FloatSize(boxWidth, boxHeight));
        float radiusX = ellipse->floatValueForRadiusInBox(ellipse->radiusX(), center.x(), boxWidth);
        float radiusY = ellipse->floatValueForRadiusInBox(ellipse->radiusY(), center.y(), boxHeight);
        FloatPoint logicalCenter = physicalPointToLogical(center, logicalBoxSize.height().toFloat(), writingMode);

        shape = createEllipseShape(logicalCenter, FloatSize(radiusX, radiusY));
        break;
    }

    case BasicShape::BasicShapePolygonType: {
        const BasicShapePolygon* polygon = toBasicShapePolygon(basicShape);
        const Vector<Length>& values = polygon->values();
        size_t valuesSize = values.size();
        ASSERT(!(valuesSize % 2));
        OwnPtr<Vector<FloatPoint>> vertices = adoptPtr(new Vector<FloatPoint>(valuesSize / 2));
        for (unsigned i = 0; i < valuesSize; i += 2) {
            FloatPoint vertex(
                floatValueForLength(values.at(i), boxWidth),
                floatValueForLength(values.at(i + 1), boxHeight));
            (*vertices)[i / 2] = physicalPointToLogical(vertex, logicalBoxSize.height().toFloat(), writingMode);
        }
        shape = createPolygonShape(vertices.release(), polygon->windRule());
        break;
    }

    case BasicShape::BasicShapeInsetType: {
        const BasicShapeInset& inset = *toBasicShapeInset(basicShape);
        float left = floatValueForLength(inset.left(), boxWidth);
        float top = floatValueForLength(inset.top(), boxHeight);
        float right = floatValueForLength(inset.right(), boxWidth);
        float bottom = floatValueForLength(inset.bottom(), boxHeight);
        FloatRect rect(left, top, std::max<float>(boxWidth - left - right, 0), std::max<float>(boxHeight - top - bottom, 0));
        FloatRect logicalRect = physicalRectToLogical(rect, logicalBoxSize.height().toFloat(), writingMode);

        FloatSize boxSize(boxWidth, boxHeight);
        FloatSize topLeftRadius = physicalSizeToLogical(floatSizeForLengthSize(inset.topLeftRadius(), boxSize), writingMode);
        FloatSize topRightRadius = physicalSizeToLogical(floatSizeForLengthSize(inset.topRightRadius(), boxSize), writingMode);
        FloatSize bottomLeftRadius = physicalSizeToLogical(floatSizeForLengthSize(inset.bottomLeftRadius(), boxSize), writingMode);
        FloatSize bottomRightRadius = physicalSizeToLogical(floatSizeForLengthSize(inset.bottomRightRadius(), boxSize), writingMode);
        FloatRoundedRect::Radii cornerRadii(topLeftRadius, topRightRadius, bottomLeftRadius, bottomRightRadius);

        FloatRoundedRect finalRect(logicalRect, cornerRadii);
        finalRect.constrainRadii();

        shape = createInsetShape(finalRect);
        break;
    }

    default:
        ASSERT_NOT_REACHED();
    }

    shape->m_writingMode = writingMode;
    shape->m_margin = margin;

    return shape.release();
}
Beispiel #30
0
ExtendedUserDataDemo::ExtendedUserDataDemo( hkDemoEnvironment* env) 
	: hkDefaultPhysicsDemo(env) 
{
	//
	// Setup the camera
	//
	{
		hkVector4 from(3.0f, 4.0f, 8.0f);
		hkVector4 to(0.0f, 1.0f, 0.0f);
		hkVector4 up(0.0f, 1.0f, 0.0f);
		setupDefaultCameras( env, from, to, up );
	}


	//
	// Create the world
	//
	{
		hkpWorldCinfo info;
		info.setupSolverInfo(hkpWorldCinfo::SOLVER_TYPE_4ITERS_MEDIUM); 
		info.setBroadPhaseWorldSize( 100.0f );
		info.m_simulationType = info.SIMULATION_TYPE_CONTINUOUS;

		// Turn off deactivation so we can see continuous contact point processing
		info.m_enableDeactivation = false;
		m_world = new hkpWorld( info );
		m_world->lock();

		setupGraphics();
	}

	//
	// Register the box-box collision agent
	//
	{
		hkpAgentRegisterUtil::registerAllAgents(m_world->getCollisionDispatcher());
	}




	//
	// Create the floor
	//
	{
		hkpRigidBodyCinfo info;
		hkVector4 fixedBoxSize(5.0f, 0.5f , 5.0f );
		hkpBoxShape* fixedBoxShape = new hkpBoxShape( fixedBoxSize , 0 );

		info.m_shape = fixedBoxShape;
		info.m_motionType = hkpMotion::MOTION_FIXED;
		info.m_position.setZero4();

		// Add some bounce.
		info.m_restitution = 0.8f;
		info.m_friction = 1.0f;
		
		// Force this to collide on PPU and raise all callbacks
		info.m_forceCollideOntoPpu = true;

		// Force this to collide on PPU and raise all callbacks
		info.m_forceCollideOntoPpu = true;

		// Create fixed box
		hkpRigidBody* floor = new hkpRigidBody(info);
		m_world->addEntity(floor);

		floor->removeReference();
		fixedBoxShape->removeReference();
	}

	// For this demo we simply have two box shapes which are constructed in the usual manner using a hkpRigidBodyCinfo 'blueprint'.
	// The dynamic box creation code in presented below. There are two key things to note in this example;
	// the 'm_restitution' member variable, which we have explicitly set to value of 0.9.
	// The restitution is over twice the default value of
	// 0.4 and is set to give the box (the floor has been set likewise) a more 'bouncy' nature. 

	//
	// Create a moving object
	//
	{
		hkpRigidBodyCinfo multiSpheresInfo;
		hkVector4 boxSize( .5f, .5f ,.5f );		

		// Build listShape of spheres
		// 
		{
			hkpSphereShape* sphere = new hkpSphereShape(0.2f);
			hkReal size = 0.5f;
			hkpShape* shapes[8];
			for (int i = 0; i < 8; i++)
			{
				hkReal px = i&0x1 ? -size : size;
				hkReal py = i&0x2 ? -size : size;
				hkReal pz = i&0x4 ? -size : size;

				hkVector4 translation(px, py, pz);
				
				shapes[i] = new hkpConvexTranslateShape(sphere, translation);

			}
			sphere->removeReference();

			hkpListShape* list = new hkpListShape(shapes, 8);
			for(int i = 0; i < 8; i++)
			{
				shapes[i]->removeReference();
			}

			multiSpheresInfo.m_shape = list;
		}




		// Compute the box inertia tensor
		hkpInertiaTensorComputer::setShapeVolumeMassProperties( multiSpheresInfo.m_shape, 1.0f, multiSpheresInfo );

		multiSpheresInfo.m_qualityType = HK_COLLIDABLE_QUALITY_CRITICAL;
		multiSpheresInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA;

		// Place the box so it bounces interestingly
		multiSpheresInfo.m_position.set(0.0f, 5.0f, 0.0f);
		hkVector4 axis(1.0f, 2.0f, 3.0f);
		axis.normalize3();
		multiSpheresInfo.m_rotation.setAxisAngle(axis, -0.7f);

		// Add some bounce.
		multiSpheresInfo.m_restitution = 0.5f;
		multiSpheresInfo.m_friction = 0.1f;
		multiSpheresInfo.m_numUserDatasInContactPointProperties = 3;

		hkpRigidBody* multiSphereRigidBody = new hkpRigidBody( multiSpheresInfo );

		// remove reference from boxShape since rigid body "owns" it
		multiSpheresInfo.m_shape->removeReference();

		m_world->addEntity( multiSphereRigidBody );
		multiSphereRigidBody->removeReference();
		
		// Add the collision event listener to the rigid body
		m_listener = new MyExtendedUserDataListener( multiSphereRigidBody );
	}

	m_world->unlock();

}