C++ SimdVector3::getX方法代码示例

void	GLDebugDrawer::DrawLine(const SimdVector3& from,const SimdVector3& to,const SimdVector3& color)
{
	if (m_debugMode > 0)
	{
		glBegin(GL_LINES);
		glColor3f(color.getX(), color.getY(), color.getZ());
		glVertex3d(from.getX(), from.getY(), from.getZ());
		glVertex3d(to.getX(), to.getY(), to.getZ());
		glEnd();
	}
}

void AxisSweep3::Quantize(unsigned short* out, const SimdPoint3& point, int isMax) const
{
	SimdPoint3 clampedPoint(point);
	/*
	if (isMax)
		clampedPoint += SimdVector3(10,10,10);
	else
	{
		clampedPoint -= SimdVector3(10,10,10);
	}
	*/


	clampedPoint.setMax(m_worldAabbMin);
	clampedPoint.setMin(m_worldAabbMax);

	SimdVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
	out[0] = (unsigned short)(((int)v.getX() & 0xfffc) | isMax);
	out[1] = (unsigned short)(((int)v.getY() & 0xfffc) | isMax);
	out[2] = (unsigned short)(((int)v.getZ() & 0xfffc) | isMax);
	
}

bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
{
	// Run GJK on the enlarged shapes to obtain a simplex of the enlarged CSO

	SimdVector3 v( 1, 0, 0 );
	SimdScalar squaredDistance = SIMD_INFINITY;

	SimdScalar delta = 0.f;

	simplexSolver.reset();

	int nbIterations = 0;

	while ( true )
	{
		EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Warning : v has zero magnitude!" );

		SimdVector3 seperatingAxisInA = -v * m_transformA.getBasis();
		SimdVector3 seperatingAxisInB =  v * m_transformB.getBasis();

		SimdVector3 pInA = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
		SimdVector3 qInB = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );

		SimdPoint3  pWorld = m_transformA( pInA );
		SimdPoint3  qWorld = m_transformB( qInB );

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

		EPA_DEBUG_ASSERT( ( delta <= 0 ) ,"Shapes are disjoint, EPA should have never    been called!" );
		if ( delta > 0.f )
			return false;

		EPA_DEBUG_ASSERT( !simplexSolver.inSimplex( w ) ,"Shapes are disjoint, EPA should have never been called!" );
		if (simplexSolver.inSimplex( w ))
			return false;

		// Add support point to simplex
		simplexSolver.addVertex( w, pWorld, qWorld );

		bool closestOk = simplexSolver.closest( v );
		EPA_DEBUG_ASSERT( closestOk ,"Shapes are disjoint, EPA should have never been called!" );
		if (!closestOk)
			return false;
		
		SimdScalar prevVSqrd = squaredDistance;
		squaredDistance = v.length2();

		// Is v converging to v(A-B) ?
		EPA_DEBUG_ASSERT( ( ( prevVSqrd - squaredDistance ) > SIMD_EPSILON * prevVSqrd ) ,
				"Shapes are disjoint, EPA should have never been called!" );
		if (( ( prevVSqrd - squaredDistance ) <= SIMD_EPSILON * prevVSqrd ))
			return false;

		if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
		{
			break;
		}

		++nbIterations;
	}

	SimdPoint3 simplexPoints[ 5 ];
	SimdPoint3 wSupportPointsOnA[ 5 ];
	SimdPoint3 wSupportPointsOnB[ 5 ];

	int nbSimplexPoints = simplexSolver.getSimplex( wSupportPointsOnA, wSupportPointsOnB, simplexPoints );

	// nbSimplexPoints can't be one because cases where the origin is on the boundary are handled
	// by hybrid penetration depth
	EPA_DEBUG_ASSERT( ( ( nbSimplexPoints > 1 ) ,( nbSimplexPoints <= 4 ) ) ,
		    "Hybrid Penetration Depth algorithm failed!" );

	int nbPolyhedronPoints = nbSimplexPoints;

#ifndef EPA_POLYHEDRON_USE_PLANES
	int initTetraIndices[ 4 ] = { 0, 1, 2, 3 };
#endif

	//	Prepare initial polyhedron to start EPA from
	if ( nbSimplexPoints == 1 )
	{
		return false;
	}
	else if ( nbSimplexPoints == 2 )
	{
		// We have a line segment inside the CSO that contains the origin
		// Create an hexahedron ( two tetrahedron glued together ) by adding 3 new points

		SimdVector3 d = simplexPoints[ 0 ] - simplexPoints[ 1 ];
		d.normalize();

		SimdVector3 v1;
		SimdVector3 v2;
		SimdVector3 v3;

		SimdVector3 e1;

		SimdScalar absx = abs( d.getX() );
		SimdScalar absy = abs( d.getY() );
//.........这里部分代码省略.........

void clientMouseFunc(int button, int state, int x, int y)
{
	//printf("button %i, state %i, x=%i,y=%i\n",button,state,x,y);
	//button 0, state 0 means left mouse down

	SimdVector3 rayTo = GetRayTo(x,y);

	switch (button)
	{
	case 2:
		{
			if (state==0)
			{
				shootBox(rayTo);
			}
			break;
		};
	case 1:
		{
			if (state==0)
			{
				//apply an impulse
				if (physicsEnvironmentPtr)
				{
					float hit[3];
					float normal[3];
					PHY_IPhysicsController* hitObj = physicsEnvironmentPtr->rayTest(0,eye[0],eye[1],eye[2],rayTo.getX(),rayTo.getY(),rayTo.getZ(),hit[0],hit[1],hit[2],normal[0],normal[1],normal[2]);
					if (hitObj)
					{
						CcdPhysicsController* physCtrl = static_cast<CcdPhysicsController*>(hitObj);
						RigidBody* body = physCtrl->GetRigidBody();
						if (body)
						{
							body->SetActivationState(ACTIVE_TAG);
							SimdVector3 impulse = rayTo;
							impulse.normalize();
							float impulseStrength = 10.f;
							impulse *= impulseStrength;
							SimdVector3 relPos(
								hit[0] - body->getCenterOfMassPosition().getX(),						
								hit[1] - body->getCenterOfMassPosition().getY(),
								hit[2] - body->getCenterOfMassPosition().getZ());

							body->applyImpulse(impulse,relPos);
						}

					}

				}

			} else
			{

			}
			break;	
		}
	case 0:
		{
			if (state==0)
			{
				//add a point to point constraint for picking
				if (physicsEnvironmentPtr)
				{
					float hit[3];
					float normal[3];
					PHY_IPhysicsController* hitObj = physicsEnvironmentPtr->rayTest(0,eye[0],eye[1],eye[2],rayTo.getX(),rayTo.getY(),rayTo.getZ(),hit[0],hit[1],hit[2],normal[0],normal[1],normal[2]);
					if (hitObj)
					{

						CcdPhysicsController* physCtrl = static_cast<CcdPhysicsController*>(hitObj);
						RigidBody* body = physCtrl->GetRigidBody();

						if (body)
						{
							pickedBody = body;
							pickedBody->SetActivationState(DISABLE_DEACTIVATION);

							SimdVector3 pickPos(hit[0],hit[1],hit[2]);

							SimdVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;

							gPickingConstraintId = physicsEnvironmentPtr->createConstraint(physCtrl,0,PHY_POINT2POINT_CONSTRAINT,
								localPivot.getX(),
								localPivot.getY(),
								localPivot.getZ(),
								0,0,0);
							//printf("created constraint %i",gPickingConstraintId);

							//save mouse position for dragging
							gOldPickingPos = rayTo;


							SimdVector3 eyePos(eye[0],eye[1],eye[2]);

							gOldPickingDist  = (pickPos-eyePos).length();

							Point2PointConstraint* p2p = static_cast<Point2PointConstraint*>(physicsEnvironmentPtr->getConstraintById(gPickingConstraintId));
							if (p2p)
							{
								//very weak constraint for picking
//.........这里部分代码省略.........

//to be implemented by the demo
void renderme()
{
	debugDrawer.SetDebugMode(getDebugMode());

	//render the hinge axis
	if (createConstraint)
	{
		SimdVector3 color(1,0,0);
		SimdVector3 dirLocal(0,1,0);
		SimdVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS);
		SimdVector3 pivotInB(-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS);
		SimdVector3 from = physObjects[1]->GetRigidBody()->getCenterOfMassTransform()(pivotInA);
		SimdVector3 fromB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform()(pivotInB);
		SimdVector3 dirWorldA = physObjects[1]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ;
		SimdVector3 dirWorldB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ;
		debugDrawer.DrawLine(from,from+dirWorldA,color);
		debugDrawer.DrawLine(fromB,fromB+dirWorldB,color);
	}

	float m[16];
	int i;


	if (getDebugMode() & IDebugDraw::DBG_DisableBulletLCP)
	{
		//don't use Bullet, use quickstep
		physicsEnvironmentPtr->setSolverType(0);
	} else
	{
		//Bullet LCP solver
		physicsEnvironmentPtr->setSolverType(1);
	}

	if (getDebugMode() & IDebugDraw::DBG_EnableCCD)
	{
		physicsEnvironmentPtr->setCcdMode(3);
	} else
	{
		physicsEnvironmentPtr->setCcdMode(0);
	}


	bool isSatEnabled = (getDebugMode() & IDebugDraw::DBG_EnableSatComparison);

	physicsEnvironmentPtr->EnableSatCollisionDetection(isSatEnabled);


#ifdef USE_HULL
	//some testing code for SAT
	if (isSatEnabled)
	{
		for (int s=0;s<numShapes;s++)
		{
			CollisionShape* shape = shapePtr[s];

			if (shape->IsPolyhedral())
			{
				PolyhedralConvexShape* polyhedron = static_cast<PolyhedralConvexShape*>(shape);
				if (!polyhedron->m_optionalHull)
				{
					//first convert vertices in 'Point3' format
					int numPoints = polyhedron->GetNumVertices();
					Point3* points = new Point3[numPoints+1];
					//first 4 points should not be co-planar, so add central point to satisfy MakeHull
					points[0] = Point3(0.f,0.f,0.f);

					SimdVector3 vertex;
					for (int p=0;p<numPoints;p++)
					{
						polyhedron->GetVertex(p,vertex);
						points[p+1] = Point3(vertex.getX(),vertex.getY(),vertex.getZ());
					}

					Hull* hull = Hull::MakeHull(numPoints+1,points);
					polyhedron->m_optionalHull = hull;
				}

			}
		}

	}
#endif //USE_HULL


	for (i=0;i<numObjects;i++)
	{
		SimdTransform transA;
		transA.setIdentity();

		float pos[3];
		float rot[4];

		ms[i].getWorldPosition(pos[0],pos[1],pos[2]);
		ms[i].getWorldOrientation(rot[0],rot[1],rot[2],rot[3]);

		SimdQuaternion q(rot[0],rot[1],rot[2],rot[3]);
		transA.setRotation(q);

		SimdPoint3 dpos;
//.........这里部分代码省略.........