C++ SimdTransform::getBasis方法代码示例

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);
}

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;
		}
//.........这里部分代码省略.........

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:
		{
//.........这里部分代码省略.........

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;
	}
    }
    
//.........这里部分代码省略.........

//.........这里部分代码省略.........
	
	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()))