コード例 #1
0
Transform	GetTransformFromSimdTransform(const SimdTransform& trans)
{
			//const SimdVector3& rowA0 = trans.getBasis().getRow(0);
			////const SimdVector3& rowA1 = trans.getBasis().getRow(1);
			//const SimdVector3& rowA2 = trans.getBasis().getRow(2);

			SimdVector3 rowA0 = trans.getBasis().getColumn(0);
			SimdVector3 rowA1 = trans.getBasis().getColumn(1);
			SimdVector3 rowA2 = trans.getBasis().getColumn(2);


			Vector3	x(rowA0.getX(),rowA0.getY(),rowA0.getZ());
			Vector3	y(rowA1.getX(),rowA1.getY(),rowA1.getZ());
			Vector3	z(rowA2.getX(),rowA2.getY(),rowA2.getZ());
			
			Matrix33 ornA(x,y,z);
	
			Point3 transA(
				trans.getOrigin().getX(),
				trans.getOrigin().getY(),
				trans.getOrigin().getZ());

			return Transform(ornA,transA);
}
コード例 #2
0
bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexSolver,
												ConvexShape* pConvexA, ConvexShape* pConvexB,
												const SimdTransform& transformA, const SimdTransform& transformB,
												SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
											    SimdScalar& penDepth, SimdVector3& v )
{
	SimdScalar squaredDistance = SIMD_INFINITY;
	SimdScalar delta = 0.f;

	const SimdScalar margin     = pConvexA->GetMargin() + pConvexB->GetMargin();
	const SimdScalar marginSqrd = margin * margin;

	simplexSolver.reset();

	int nbIterations = 0;

	while ( true )
	{
		assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" );

		SimdVector3 seperatingAxisInA = -v * transformA.getBasis();
		SimdVector3 seperatingAxisInB =  v * transformB.getBasis();

		SimdVector3 pInA = pConvexA->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA );
		SimdVector3 qInB = pConvexB->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB );

		SimdPoint3  pWorld = transformA( pInA );
		SimdPoint3  qWorld = transformB( qInB );

		SimdVector3 w = pWorld - qWorld;
		delta = v.dot( w );

		// potential exit, they don't overlap
		if ( ( delta > 0 ) && ( ( delta * delta / squaredDistance ) > marginSqrd ) )
		{
			// Convex shapes do not overlap
			// Returning true means that Hybrid's result is ok and there's no need to run EPA
			penDepth = 0;
			return true;
		}

		//exit 0: the new point is already in the simplex, or we didn't come any closer
		if ( ( squaredDistance - delta <= squaredDistance * g_GJKMaxRelErrorSqrd ) || simplexSolver.inSimplex( w ) )
		{
			simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );

			assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
			SimdScalar vLength = sqrt( squaredDistance );

			wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
			wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );

			penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;

			// Returning true means that Hybrid's result is ok and there's no need to run EPA
			return true;
		}

		//add current vertex to simplex
		simplexSolver.addVertex( w, pWorld, qWorld );

		//calculate the closest point to the origin (update vector v)
		if ( !simplexSolver.closest( v ) )
		{
			simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );

			assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
			SimdScalar vLength = sqrt( squaredDistance );

			wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
			wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );

			penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;

			// Returning true means that Hybrid's result is ok and there's no need to run EPA
			return true;
		}

		SimdScalar previousSquaredDistance = squaredDistance;
		squaredDistance = v.length2();

		//are we getting any closer ?
		if ( previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance ) 
		{ 
			simplexSolver.backup_closest( v );
			squaredDistance = v.length2();

			simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );

			assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
			SimdScalar vLength = sqrt( squaredDistance );

			wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
			wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );

			penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;

			// Returning true means that Hybrid's result is ok and there's no need to run EPA
			return true;
		}

		if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
		{
			// Convex Shapes intersect - we need to run EPA
			// Returning false means that Hybrid couldn't do anything for us
			// and that we need to run EPA to calculate the pen depth
			return false;
		}

		++nbIterations;
	}
}
コード例 #3
0
int			CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
													float pivotX,float pivotY,float pivotZ,
													float axisX,float axisY,float axisZ)
{


	CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
	CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;

	RigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
	RigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;

	ASSERT(rb0);

	SimdVector3 pivotInA(pivotX,pivotY,pivotZ);
	SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
	SimdVector3 axisInA(axisX,axisY,axisZ);
	SimdVector3 axisInB = rb1 ? 
		(rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) : 
	rb0->getCenterOfMassTransform().getBasis() * axisInA;

	bool angularOnly = false;

	switch (type)
	{
	case PHY_POINT2POINT_CONSTRAINT:
		{

			Point2PointConstraint* p2p = 0;

			if (rb1)
			{
				p2p = new Point2PointConstraint(*rb0,
					*rb1,pivotInA,pivotInB);
			} else
			{
				p2p = new Point2PointConstraint(*rb0,
					pivotInA);
			}

			m_constraints.push_back(p2p);
			p2p->SetUserConstraintId(gConstraintUid++);
			p2p->SetUserConstraintType(type);
			//64 bit systems can't cast pointer to int. could use size_t instead.
			return p2p->GetUserConstraintId();

			break;
		}

	case PHY_GENERIC_6DOF_CONSTRAINT:
		{
			Generic6DofConstraint* genericConstraint = 0;

			if (rb1)
			{
				SimdTransform frameInA;
				SimdTransform frameInB;
				
				SimdVector3 axis1, axis2;
				SimdPlaneSpace1( axisInA, axis1, axis2 );

				frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
					                          axisInA.y(), axis1.y(), axis2.y(),
											  axisInA.z(), axis1.z(), axis2.z() );

	
				SimdPlaneSpace1( axisInB, axis1, axis2 );
				frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
					                          axisInB.y(), axis1.y(), axis2.y(),
											  axisInB.z(), axis1.z(), axis2.z() );

				frameInA.setOrigin( pivotInA );
				frameInB.setOrigin( pivotInB );

				genericConstraint = new Generic6DofConstraint(
					*rb0,*rb1,
					frameInA,frameInB);


			} else
			{
				// TODO: Implement single body case...

			}
			

			m_constraints.push_back(genericConstraint);
			genericConstraint->SetUserConstraintId(gConstraintUid++);
			genericConstraint->SetUserConstraintType(type);
			//64 bit systems can't cast pointer to int. could use size_t instead.
			return genericConstraint->GetUserConstraintId();

			break;
		}
	case PHY_ANGULAR_CONSTRAINT:
		angularOnly = true;


	case PHY_LINEHINGE_CONSTRAINT:
		{
			HingeConstraint* hinge = 0;

			if (rb1)
			{
				hinge = new HingeConstraint(
					*rb0,
					*rb1,pivotInA,pivotInB,axisInA,axisInB);


			} else
			{
				hinge = new HingeConstraint(*rb0,
					pivotInA,axisInA);

			}
			hinge->setAngularOnly(angularOnly);

			m_constraints.push_back(hinge);
			hinge->SetUserConstraintId(gConstraintUid++);
			hinge->SetUserConstraintType(type);
			//64 bit systems can't cast pointer to int. could use size_t instead.
			return hinge->GetUserConstraintId();
			break;
		}
#ifdef NEW_BULLET_VEHICLE_SUPPORT

	case PHY_VEHICLE_CONSTRAINT:
		{
			RaycastVehicle::VehicleTuning* tuning = new RaycastVehicle::VehicleTuning();
			RigidBody* chassis = rb0;
			DefaultVehicleRaycaster* raycaster = new DefaultVehicleRaycaster(this,ctrl0);
			RaycastVehicle* vehicle = new RaycastVehicle(*tuning,chassis,raycaster);
			WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
			m_wrapperVehicles.push_back(wrapperVehicle);
			vehicle->SetUserConstraintId(gConstraintUid++);
			vehicle->SetUserConstraintType(type);
			return vehicle->GetUserConstraintId();

			break;
		};
#endif //NEW_BULLET_VEHICLE_SUPPORT

	default:
		{
		}
	};

	//RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB

	return 0;

}
コード例 #4
0
bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSolver,
			ConvexShape* convexA,ConvexShape* convexB,
			const SimdTransform& transformA,const SimdTransform& transformB,
			SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb)
{
	
    int num_verts = simplexSolver.getSimplex(pBuf, qBuf, yBuf);

    switch (num_verts) 
	{
	case 1:
	    // Touching contact. Yes, we have a collision,
	    // but no penetration.
	    return false;
	case 2:	
	{
	    // We have a line segment inside the Minkowski sum containing the
	    // origin. Blow it up by adding three additional support points.
	    
	    SimdVector3 dir  = (yBuf[1] - yBuf[0]).normalized();
	    int        axis = dir.furthestAxis();
	    
	    static SimdScalar sin_60 = 0.8660254037f;//84438646763723170752941.22474487f;//13915890490986420373529;//
	    
	    SimdQuaternion rot(dir[0] * sin_60, dir[1] * sin_60, dir[2] * sin_60, SimdScalar(0.5));
	    SimdMatrix3x3 rot_mat(rot);
	    
	    SimdVector3 aux1 = dir.cross(SimdVector3(axis == 0, axis == 1, axis == 2));
	    SimdVector3 aux2 = rot_mat * aux1;
	    SimdVector3 aux3 = rot_mat * aux2;
	    
	    pBuf[2] = transformA(convexA->LocalGetSupportingVertex(aux1*transformA.getBasis()));
		qBuf[2] = transformB(convexB->LocalGetSupportingVertex((-aux1)*transformB.getBasis()));
	    yBuf[2] = pBuf[2] - qBuf[2];
	    
	    pBuf[3] = transformA(convexA->LocalGetSupportingVertex(aux2*transformA.getBasis()));
		qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-aux2)*transformB.getBasis()));
	    yBuf[3] = pBuf[3] - qBuf[3];
	    
		pBuf[4] = transformA(convexA->LocalGetSupportingVertex(aux3*transformA.getBasis()));
		qBuf[4] = transformB(convexB->LocalGetSupportingVertex((-aux3)*transformB.getBasis()));
	    yBuf[4] = pBuf[4] - qBuf[4];
	    
	    if (originInTetrahedron(yBuf[0], yBuf[2], yBuf[3], yBuf[4])) 
		{
			pBuf[1] = pBuf[4];
			qBuf[1] = qBuf[4];
			yBuf[1] = yBuf[4];
	    }
	    else if (originInTetrahedron(yBuf[1], yBuf[2], yBuf[3], yBuf[4])) 
		{
			pBuf[0] = pBuf[4];
			qBuf[0] = qBuf[4];
			yBuf[0] = yBuf[4];
	    } 
	    else 
		{
			// Origin not in initial polytope
			return false;
	    }
	    
	    num_verts = 4;
	    
	    break;
	}
	case 3: 
	{
	    // We have a triangle inside the Minkowski sum containing
	    // the origin. First blow it up.
	    
	    SimdVector3 v1     = yBuf[1] - yBuf[0];
	    SimdVector3 v2     = yBuf[2] - yBuf[0];
	    SimdVector3 vv     = v1.cross(v2);
	    
		pBuf[3] = transformA(convexA->LocalGetSupportingVertex(vv*transformA.getBasis()));
		qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-vv)*transformB.getBasis()));
	    yBuf[3] = pBuf[3] - qBuf[3];
		pBuf[4] = transformA(convexA->LocalGetSupportingVertex((-vv)*transformA.getBasis()));
		qBuf[4] = transformB(convexB->LocalGetSupportingVertex(vv*transformB.getBasis()));
	    yBuf[4] = pBuf[4] - qBuf[4];
	    
	   
	    if (originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[4])) 
		{
			pBuf[3] = pBuf[4];
			qBuf[3] = qBuf[4];
			yBuf[3] = yBuf[4];
	    }
	    else if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3]))
		{ 
			// Origin not in initial polytope
			return false;
	    }
	    
	    num_verts = 4;
	    
	    break;
	}
    }
    
    // We have a tetrahedron inside the Minkowski sum containing
    // the origin (if GJK did it's job right ;-)
      
    
    if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3])) 
	{
		//	assert(false);
		return false;
	}
    
	num_facets = 0;
    freeFacet = 0;

    ReplaceMeFacet *f0 = addFacet(0, 1, 2, SimdScalar(0.0), SIMD_INFINITY);
    ReplaceMeFacet *f1 = addFacet(0, 3, 1, SimdScalar(0.0), SIMD_INFINITY);
    ReplaceMeFacet *f2 = addFacet(0, 2, 3, SimdScalar(0.0), SIMD_INFINITY);
    ReplaceMeFacet *f3 = addFacet(1, 3, 2, SimdScalar(0.0), SIMD_INFINITY);
    
    if (!f0 || f0->getDist2() == SimdScalar(0.0) ||
		!f1 || f1->getDist2() == SimdScalar(0.0) ||
		!f2 || f2->getDist2() == SimdScalar(0.0) ||
		!f3 || f3->getDist2() == SimdScalar(0.0)) 
	{
		return false;
    }
    
    f0->link(0, f1, 2);
    f0->link(1, f3, 2);
    f0->link(2, f2, 0);
    f1->link(0, f2, 2);
    f1->link(1, f3, 0);
    f2->link(1, f3, 1);
    
    if (num_facets == 0) 
	{
		return false;
    }
    
    // at least one facet on the heap.	
    
    ReplaceMeEdgeBuffer edgeBuffer(20);

    ReplaceMeFacet *facet = 0;
    
    SimdScalar upper_bound2 = SIMD_INFINITY; 	
    
    do {
        facet = facetHeap[0];
        std::pop_heap(&facetHeap[0], &facetHeap[num_facets], myFacetComp);
        --num_facets;
		
		if (!facet->isObsolete()) 
		{
			assert(facet->getDist2() > SimdScalar(0.0));
			
			if (num_verts == MaxSupportPoints)
			{
#ifdef DEBUG
				std::cout << "Ouch, no convergence!!!" << std::endl;
#endif 
				ASSERT_MESSAGE(false,"Error: pendepth calc failed");	
				break;
			}
			
			pBuf[num_verts] = transformA(convexA->LocalGetSupportingVertex((facet->getClosest())*transformA.getBasis()));
			qBuf[num_verts] = transformB(convexB->LocalGetSupportingVertex((-facet->getClosest())*transformB.getBasis()));
			yBuf[num_verts] = pBuf[num_verts] - qBuf[num_verts];
			

			int index = num_verts++;
			SimdScalar far_dist2 = yBuf[index].dot(facet->getClosest());
			

			// Make sure the support mapping is OK.
			//assert(far_dist2 > SimdScalar(0.0));
			
			//
			// this is to avoid problems with implicit-sphere-touching contact
			//
			if (far_dist2 < SimdScalar(0.0))
			{
				return false;
			}

			GEN_set_min(upper_bound2, (far_dist2 * far_dist2) / facet->getDist2());
			
			if (upper_bound2 <= ReplaceMeAccuracy::depth_tolerance * facet->getDist2()
#define CHECK_NEW_SUPPORT
#ifdef CHECK_NEW_SUPPORT
				|| yBuf[index] == yBuf[(*facet)[0]] 
				|| yBuf[index] == yBuf[(*facet)[1]]
				|| yBuf[index] == yBuf[(*facet)[2]]
#endif
				) 
			{
				break;
			}
			
			// Compute the silhouette cast by the new vertex
			// Note that the new vertex is on the positive side
			// of the current facet, so the current facet is will
			// not be in the convex hull. Start local search
			// from this facet.
			
			facet->silhouette(yBuf[index], edgeBuffer);
			
			if (edgeBuffer.empty()) 
			{
				return false;
			}
			
			ReplaceMeEdgeBuffer::const_iterator it = edgeBuffer.begin();
			ReplaceMeFacet *firstFacet = 
				addFacet((*it).getTarget(), (*it).getSource(),
						 index, facet->getDist2(), upper_bound2);
			
			if (!firstFacet) 
			{
				break;
			}
			
			firstFacet->link(0, (*it).getFacet(), (*it).getIndex());
			ReplaceMeFacet *lastFacet = firstFacet;
			
			++it;
			for (; it != edgeBuffer.end(); ++it) 
			{
				ReplaceMeFacet *newFacet = 
					addFacet((*it).getTarget(), (*it).getSource(),
							 index, facet->getDist2(), upper_bound2);
				
				if (!newFacet) 
				{
					break;
				}
				
				if (!newFacet->link(0, (*it).getFacet(), (*it).getIndex())) 
				{
					break;
				}
				
				if (!newFacet->link(2, lastFacet, 1)) 
				{
					break;
				}
				
				lastFacet = newFacet;				
			}
			if (it != edgeBuffer.end()) 
			{
				break;
			}
			
			firstFacet->link(2, lastFacet, 1);
		}
    }
    while (num_facets > 0 && facetHeap[0]->getDist2() <= upper_bound2);
	
#ifdef DEBUG    
    std::cout << "#facets left = " << num_facets << std::endl;
#endif
    
    v = facet->getClosest();
    pa = facet->getClosestPoint(pBuf);    
    pb = facet->getClosestPoint(qBuf);    
    return true;
}
コード例 #5
0
bool BU_CollisionPair::calcTimeOfImpact(
					const SimdTransform& fromA,
					const SimdTransform& toA,
					const SimdTransform& fromB,
					const SimdTransform& toB,
					CastResult& result)
{



	
	SimdVector3 linvelA,angvelA;
	SimdVector3 linvelB,angvelB;

	SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA);
	SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB);


	SimdVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin();
	SimdQuaternion angularMotionA(0,0,0,1.f);
	SimdVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin();
	SimdQuaternion angularMotionB(0,0,0,1);
	


	result.m_fraction = 1.f;

	SimdTransform impactTransA;
	SimdTransform impactTransB;

	int index=0;

	SimdScalar toiUnscaled=result.m_fraction;
	const SimdScalar toiUnscaledLimit = result.m_fraction;

	SimdTransform a2w;
	a2w = fromA;
	SimdTransform b2w = fromB;

/* debugging code
	{
		const int numvertsB = m_convexB->GetNumVertices();
		for (int v=0;v<numvertsB;v++)
		{
			SimdPoint3 pt;
			m_convexB->GetVertex(v,pt);
			pt = b2w * pt;
			char buf[1000];

			if (pt.y() < 0.)
			{
				sprintf(buf,"PRE ERROR (%d) %.20E %.20E %.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
				if (debugFile)
					fwrite(buf,1,strlen(buf),debugFile);
			} else
			{
				sprintf(buf,"PRE %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
				if (debugFile)
					fwrite(buf,1,strlen(buf),debugFile);

			}
		}
	}
*/


	SimdTransform b2wp = b2w;
	
	b2wp.setOrigin(b2w.getOrigin() + linearMotionB);
	b2wp.setRotation( b2w.getRotation() + angularMotionB);

	impactTransB = b2wp;
	
	SimdTransform a2wp;
	a2wp.setOrigin(a2w.getOrigin()+ linearMotionA);
	a2wp.setRotation(a2w.getRotation()+angularMotionA);

	impactTransA = a2wp;

	SimdTransform a2winv;
	a2winv = a2w.inverse();

	SimdTransform b2wpinv;
	b2wpinv = b2wp.inverse();

	SimdTransform b2winv;
	b2winv = b2w.inverse();

	SimdTransform a2wpinv;
	a2wpinv = a2wp.inverse();

		//Redon's version with concatenated transforms

	SimdTransform relative;

	relative = b2w * b2wpinv * a2wp * a2winv;

	//relative = a2winv * a2wp  * b2wpinv * b2w;

	SimdQuaternion qrel;
	relative.getBasis().getRotation(qrel);

	SimdVector3 linvel = relative.getOrigin();

	if (linvel.length() < SCREWEPSILON)
	{
		linvel.setValue(0.,0.,0.);
	}
	SimdVector3 angvel;
	angvel[0] = 2.f * SimdAsin (qrel[0]);
	angvel[1] = 2.f * SimdAsin (qrel[1]);
	angvel[2] = 2.f * SimdAsin (qrel[2]);
	
	if (angvel.length() < SCREWEPSILON)
	{
		angvel.setValue(0.f,0.f,0.f);
	}

	//Redon's version with concatenated transforms
	m_screwing = BU_Screwing(linvel,angvel);
	
	SimdTransform w2s;
	m_screwing.LocalMatrix(w2s);

	SimdTransform s2w;
	s2w = w2s.inverse();

	//impactTransA = a2w;
	//impactTransB = b2w;

	bool hit = false;
	
	if (SimdFuzzyZero(m_screwing.GetS()) && SimdFuzzyZero(m_screwing.GetW()))
	{
		//W = 0 , S = 0 , no collision
		//toi = 0;
	/*	
		{
			const int numvertsB = m_convexB->GetNumVertices();
			for (int v=0;v<numvertsB;v++)
			{
				SimdPoint3 pt;
				m_convexB->GetVertex(v,pt);
				pt = impactTransB * pt;
				char buf[1000];
				
				if (pt.y() < 0.)
				{
					sprintf(buf,"EARLY POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
					if (debugFile)
						fwrite(buf,1,strlen(buf),debugFile);
				}
				else
				{
					sprintf(buf,"EARLY POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
					if (debugFile)
						fwrite(buf,1,strlen(buf),debugFile);
				}
			}
		}
	*/	
		
		return false;//don't continue moving within epsilon
	}

#define EDGEEDGE
#ifdef EDGEEDGE

	BU_EdgeEdge edgeEdge;

	//for all edged in A check agains all edges in B
	for (int ea = 0;ea < m_convexA->GetNumEdges();ea++)
	{
		SimdPoint3 pA0,pA1;

		m_convexA->GetEdge(ea,pA0,pA1);

		pA0= a2w * pA0;//in world space
		pA0 = w2s * pA0;//in screwing space

		pA1= a2w * pA1;//in world space
		pA1 = w2s * pA1;//in screwing space

		int numedgesB = m_convexB->GetNumEdges();
		for (int eb = 0; eb < numedgesB;eb++)
		{
			{
				SimdPoint3 pB0,pB1;
				m_convexB->GetEdge(eb,pB0,pB1);

				pB0= b2w * pB0;//in world space
				pB0 = w2s * pB0;//in screwing space

				pB1= b2w * pB1;//in world space
				pB1 = w2s * pB1;//in screwing space


				SimdScalar lambda,mu;
				
				toiUnscaled = 1.;

				SimdVector3 edgeDirA(pA1-pA0);
				SimdVector3 edgeDirB(pB1-pB0);

				if (edgeEdge.GetTimeOfImpact(m_screwing,pA0,edgeDirA,pB0,edgeDirB,toiUnscaled,lambda,mu))
				{
					//printf("edgeedge potential hit\n");
					if (toiUnscaled>=0)
					{
						if (toiUnscaled < toiUnscaledLimit)							
						{
		
							//inside check is already done by checking the mu and gamma !

							SimdPoint3 vtx  = pA0+lambda * (pA1-pA0);
							SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
							
							SimdPoint3 hitptWorld =   s2w * hitpt;
							{

								if (toiUnscaled < result.m_fraction)
									result.m_fraction = toiUnscaled;

								hit = true;

								SimdVector3 hitNormal = edgeDirB.cross(edgeDirA);
								
								hitNormal = m_screwing.InBetweenVector(hitNormal,toiUnscaled);
							

								hitNormal.normalize();
								
								//an approximated normal can be calculated by taking the cross product of both edges
								//take care of the sign !
								
								SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
						
								SimdScalar dist = m_screwing.GetU().dot(hitNormalWorld);
								if (dist > 0)
									hitNormalWorld *= -1;
								
								//todo: this is the wrong point, because b2winv is still at begin of motion
								// not at time-of-impact location!
								//bhitpt = b2winv * hitptWorld;

//								m_manifold.SetContactPoint(BUM_FeatureEdgeEdge,index,ea,eb,hitptWorld,hitNormalWorld);
							}
					
						}
					}
				}
			}

			index++;
		}
	};
#endif //EDGEEDGE

#define VERTEXFACE
#ifdef VERTEXFACE

	// for all vertices in A, for each face in B,do vertex-face
	{
		const int numvertsA = m_convexA->GetNumVertices();
		for (int v=0;v<numvertsA;v++)
		//int v=3;

		{
			SimdPoint3 vtx;
			m_convexA->GetVertex(v,vtx);

			vtx = a2w * vtx;//in world space
			vtx = w2s * vtx;//in screwing space

			const int numplanesB = m_convexB->GetNumPlanes();

			for (int p = 0 ; p < numplanesB; p++)
			//int p=2;
			{

				{
				
					SimdVector3 planeNorm;
					SimdPoint3 planeSupport;

					m_convexB->GetPlane(planeNorm,planeSupport,p);


					planeSupport = b2w * planeSupport;//transform to world space
					SimdVector3 planeNormWorld =  b2w.getBasis() * planeNorm;
				
					planeSupport =  w2s * planeSupport  ; //transform to screwing space
					planeNorm =  w2s.getBasis() * planeNormWorld;

					planeNorm.normalize();

					SimdScalar d = planeSupport.dot(planeNorm);
					
					SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
				
					BU_VertexPoly vtxApolyB;

					toiUnscaled = 1.;

					if ((p==2) && (v==6))
					{
//						printf("%f toiUnscaled\n",toiUnscaled);

					}
					if (vtxApolyB.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,false))
					{
					


						
						if (toiUnscaled >= 0. )
						{
							//not only collect the first point, get every contactpoint, later we have to check the
							//manifold properly!

							if (toiUnscaled <= toiUnscaledLimit)
							{
	//							printf("toiUnscaled %f\n",toiUnscaled );

								SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
								SimdVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled);

								SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
								SimdPoint3 hitptWorld = s2w * hitpt;


								hitpt = b2winv * hitptWorld;
								//vertex has to be 'within' the facet's boundary
								if (m_convexB->IsInside(hitpt,m_tolerance))
								{
//									m_manifold.SetContactPoint(BUM_FeatureVertexFace, index,v,p,hitptWorld,hitNormalWorld);
									
									if (toiUnscaled < result.m_fraction)
										result.m_fraction= toiUnscaled;
									hit = true;

								}
							}
						}
					}
					
				}

				index++;
			}
		}
	}

	//
	// for all vertices in B, for each face in A,do vertex-face
	//copy and pasted from all verts A -> all planes B so potential typos!
	//todo: make this into one method with a kind of 'swapped' logic
	//
	{
		const int numvertsB = m_convexB->GetNumVertices();
		for (int v=0;v<numvertsB;v++)
		//int v=0;

		{
			SimdPoint3 vtx;
			m_convexB->GetVertex(v,vtx);

			vtx = b2w * vtx;//in world space
/*
			
			char buf[1000];

			if (vtx.y() < 0.)
			{
				sprintf(buf,"ERROR !!!!!!!!!\n",v,vtx.x(),vtx.y(),vtx.z());
				if (debugFile)
					fwrite(buf,1,strlen(buf),debugFile);
			}
			sprintf(buf,"vertexWorld(%d) = (%.20E,%.20E,%.20E)\n",v,vtx.x(),vtx.y(),vtx.z());
			if (debugFile)
				fwrite(buf,1,strlen(buf),debugFile);

*/			
			vtx = w2s * vtx;//in screwing space

			const int numplanesA = m_convexA->GetNumPlanes();

			for (int p = 0 ; p < numplanesA; p++)
			//int p=2;
			{

				{
					SimdVector3 planeNorm;
					SimdPoint3 planeSupport;

					m_convexA->GetPlane(planeNorm,planeSupport,p);


					planeSupport = a2w * planeSupport;//transform to world space
					SimdVector3 planeNormWorld =  a2w.getBasis() * planeNorm;
				
					planeSupport =  w2s * planeSupport  ; //transform to screwing space
					planeNorm =  w2s.getBasis() * planeNormWorld;

					planeNorm.normalize();

					SimdScalar d = planeSupport.dot(planeNorm);
					
					SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
				
					BU_VertexPoly vtxBpolyA;

					toiUnscaled = 1.;

					if (vtxBpolyA.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,true))
					{
						if (toiUnscaled>=0.)
						{
							if (toiUnscaled < toiUnscaledLimit)
							{
								SimdPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled);
								SimdVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled);
								//SimdScalar len =  hitNormal.length()-1;

								//assert( SimdFuzzyZero(len) );

								
								SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
								SimdPoint3 hitptWorld = s2w * hitpt;
								hitpt = a2winv * hitptWorld;
							
							
								//vertex has to be 'within' the facet's boundary
								if (m_convexA->IsInside(hitpt,m_tolerance))
								{
									
//									m_manifold.SetContactPoint(BUM_FeatureFaceVertex,index,p,v,hitptWorld,hitNormalWorld);
									if (toiUnscaled <result.m_fraction)
										result.m_fraction = toiUnscaled;
									hit = true;
								}
							}
						
						}
					
					}
					}

			}
		
			index++;
		}
	}
	

#endif// VERTEXFACE

	//the manifold now consists of all points/normals generated by feature-pairs that have a time-of-impact within this frame
	//in addition there are contact points from previous frames
	//we have to cleanup the manifold, using an additional epsilon/tolerance
	//as long as the distance from the contactpoint (in worldspace) to both objects is within this epsilon we keep the point
	//else throw it away
	

	if (hit)
	{

		//try to avoid numerical drift on close contact
		
		if (result.m_fraction < 0.00001)
		{
//			printf("toiUnscaledMin< 0.00001\n");
			impactTransA = a2w;
			impactTransB = b2w;

		} else
		{

			//SimdScalar vel = linearMotionB.length();
			
			//todo: check this margin
			result.m_fraction *= 0.99f;

			//move B to new position
			impactTransB.setOrigin(b2w.getOrigin()+ result.m_fraction*linearMotionB);
			SimdQuaternion ornB = b2w.getRotation()+angularMotionB*result.m_fraction;
			ornB.normalize();
			impactTransB.setRotation(ornB);

			//now transform A
			SimdTransform a2s,a2b;
			a2s.mult( w2s , a2w);
			a2s= m_screwing.InBetweenTransform(a2s,result.m_fraction);
			a2s.multInverseLeft(w2s,a2s);
			a2b.multInverseLeft(b2w, a2s);

			//transform by motion B
			impactTransA.mult(impactTransB, a2b);
			//normalize rotation
			SimdQuaternion orn;
			impactTransA.getBasis().getRotation(orn);
			orn.normalize();
			impactTransA.setBasis(SimdMatrix3x3(orn));
		}
	}

/*
	{
		const int numvertsB = m_convexB->GetNumVertices();
		for (int v=0;v<numvertsB;v++)
		{
			SimdPoint3 pt;
			m_convexB->GetVertex(v,pt);
			pt = impactTransB * pt;
			char buf[1000];

			if (pt.y() < 0.)
			{
				sprintf(buf,"POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
				if (debugFile)
					fwrite(buf,1,strlen(buf),debugFile);
			}
			else
			{
				sprintf(buf,"POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
				if (debugFile)
					fwrite(buf,1,strlen(buf),debugFile);
			}
		}
	}
*/
	return hit;
}