C++ SimdVector3类代码示例

SimdVector3	PolyhedralConvexShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec0)const
{
	int i;
	SimdVector3 supVec(0,0,0);

	SimdScalar maxDot(-1e30f);

	SimdVector3 vec = vec0;
	SimdScalar lenSqr = vec.length2();
	if (lenSqr < 0.0001f)
	{
		vec.setValue(1,0,0);
	} else
	{
		float rlen = 1.f / SimdSqrt(lenSqr );
		vec *= rlen;
	}

	SimdVector3 vtx;
	SimdScalar newDot;

	for (i=0;i<GetNumVertices();i++)
	{
		GetVertex(i,vtx);
		newDot = vec.dot(vtx);
		if (newDot > maxDot)
		{
			maxDot = newDot;
			supVec = vtx;
		}
	}

	return supVec;

}

void	clientMotionFunc(int x,int y)
{

	if (gPickingConstraintId && physicsEnvironmentPtr)
	{

		//move the constraint pivot

		Point2PointConstraint* p2p = static_cast<Point2PointConstraint*>(physicsEnvironmentPtr->getConstraintById(gPickingConstraintId));
		if (p2p)
		{
			//keep it at the same picking distance

			SimdVector3 newRayTo = GetRayTo(x,y);
			SimdVector3 eyePos(eye[0],eye[1],eye[2]);
			SimdVector3 dir = newRayTo-eyePos;
			dir.normalize();
			dir *= gOldPickingDist;

			SimdVector3 newPos = eyePos + dir;
			p2p->SetPivotB(newPos);
		}

	}
}

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

SimdVector3	ConvexTriangleMeshShape::LocalGetSupportingVertex(const SimdVector3& vec)const
{
	SimdVector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);

	if ( GetMargin()!=0.f )
	{
		SimdVector3 vecnorm = vec;
		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
		{
			vecnorm.setValue(-1.f,-1.f,-1.f);
		} 
		vecnorm.normalize();
		supVertex+= GetMargin() * vecnorm;
	}
	return supVertex;
}

bool ReplaceMeFacet::computeClosest(const SimdVector3 *verts)
{
    const SimdVector3& p0 = verts[m_indices[0]]; 

    SimdVector3 v1 = verts[m_indices[1]] - p0;
    SimdVector3 v2 = verts[m_indices[2]] - p0;
    SimdScalar v1dv1 = v1.length2();
    SimdScalar v1dv2 = v1.dot(v2);
    SimdScalar v2dv2 = v2.length2();
    SimdScalar p0dv1 = p0.dot(v1); 
    SimdScalar p0dv2 = p0.dot(v2);

    m_det = v1dv1 * v2dv2 - v1dv2 * v1dv2; // non-negative
    //printf("m_det = %f\n",m_det);
    //ASSERT(m_det >= 0.f);

    if (m_det >= (SIMD_EPSILON*SIMD_EPSILON)) {	

    	m_lambda1 = p0dv2 * v1dv2 - p0dv1 * v2dv2;
	    m_lambda2 = p0dv1 * v1dv2 - p0dv2 * v1dv1; 
	
		m_closest = p0 + (m_lambda1 * v1 + m_lambda2 * v2) / m_det;
		m_dist2 = m_closest.length2();
		return true;
    }
    
    return false;
} 

///
/// Debugging method CalcClosest calculates the closest point to the origin, using m_simplexSolver
///
void	GL_Simplex1to4::CalcClosest(float* m)
{
    SimdTransform tr;
    tr.setFromOpenGLMatrix(m);



    GL_ShapeDrawer::DrawCoordSystem();

    if (m_simplexSolver)
    {
        m_simplexSolver->reset();
        bool res;

        SimdVector3 v;
        SimdPoint3 pBuf[4];
        SimdPoint3 qBuf[4];
        SimdPoint3 yBuf[4];


        for (int i=0; i<m_numVertices; i++)
        {
            v =  tr(m_vertices[i]);
            m_simplexSolver->addVertex(v,v,SimdPoint3(0.f,0.f,0.f));
            res = m_simplexSolver->closest(v);
            int res = m_simplexSolver->getSimplex(pBuf, qBuf, yBuf);

        }


        //draw v?
        glDisable(GL_LIGHTING);
        glBegin(GL_LINES);
        glColor3f(1.f, 0.f, 0.f);
        glVertex3f(0.f, 0.f, 0.f);
        glVertex3f(v.x(),v.y(),v.z());
        glEnd();

        glEnable(GL_LIGHTING);


    }

}

	virtual void InternalProcessTriangleIndex(SimdVector3* triangle,int partId,int  triangleIndex)
	{
		for (int i=0;i<3;i++)
		{
			SimdScalar dot = m_supportVecLocal.dot(triangle[i]);
			if (dot > m_maxDot)
			{
				m_maxDot = dot;
				m_supportVertexLocal = triangle[i];
			}
		}
	}

SimdVector3	GetRayTo(int x,int y)
{
	float top = 1.f;
	float bottom = -1.f;
	float nearPlane = 1.f;
	float tanFov = (top-bottom)*0.5f / nearPlane;
	float fov = 2.0 * atanf (tanFov);

	SimdVector3	rayFrom(eye[0],eye[1],eye[2]);
	SimdVector3 rayForward = -rayFrom;
	rayForward.normalize();
	float farPlane = 600.f;
	rayForward*= farPlane;

	SimdVector3 rightOffset;
	SimdVector3 vertical(0.f,1.f,0.f);
	SimdVector3 hor;
	hor = rayForward.cross(vertical);
	hor.normalize();
	vertical = hor.cross(rayForward);
	vertical.normalize();

	float tanfov = tanf(0.5f*fov);
	hor *= 2.f * farPlane * tanfov;
	vertical *= 2.f * farPlane * tanfov;
	SimdVector3 rayToCenter = rayFrom + rayForward;
	SimdVector3 dHor = hor * 1.f/float(glutScreenWidth);
	SimdVector3 dVert = vertical * 1.f/float(glutScreenHeight);
	SimdVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
	rayTo += x * dHor;
	rayTo -= y * dVert;
	return rayTo;
}

void clientKeyboard(unsigned char key, int x, int y)
{
	if ( key == 'R' || key == 'r' )
	{
		destroyShapes();

		g_shapesType[ 0 ] = randomShapeType( 0, 1 );
		g_shapesType[ 1 ] = randomShapeType( 0, 1 );

		( g_shapesType[ 0 ] == 0 ) ? createBoxShape( 0 ) : createSphereShape( 0 );
		( g_shapesType[ 1 ] == 0 ) ? createBoxShape( 1 ) : createSphereShape( 1 );

		g_shapesPenetrate = calcPenDepth();
	}
	else if ( key == 'Q' || key == 'q' )
	{
		destroyShapes();
	}
	else if ( key == 'T' || key == 't' )
	{
#ifdef DEBUG_ME
		SimdVector3 shapeAPos = g_convexShapesTransform[ 0 ].getOrigin();
		SimdVector3 shapeBPos = g_convexShapesTransform[ 1 ].getOrigin();

		SimdMatrix3x3 shapeARot = g_convexShapesTransform[ 0 ].getBasis();
		SimdMatrix3x3 shapeBRot = g_convexShapesTransform[ 1 ].getBasis();

		FILE* fp = 0;

		fopen_s( &fp, "shapes.txt", "w" );

		char str[ 256 ];
		sprintf_s( str, 256, "PosA: %f, %f, %f\nPosB: %f, %f, %f\n", shapeAPos.x(), shapeAPos.y(), shapeAPos.z(),
			shapeBPos.x(), shapeBPos.y(), shapeBPos.z() );
		fputs( str, fp );

		sprintf_s( str, 256, "RotA: %f, %f, %f\n%f, %f, %f\n%f, %f, %f\nRotB: %f, %f, %f\n%f, %f, %f\n%f, %f, %f\n\n",
			shapeARot.getRow( 0 ).x(), shapeARot.getRow( 0 ).y(), shapeARot.getRow( 0 ).z(),
			shapeARot.getRow( 1 ).x(), shapeARot.getRow( 1 ).y(), shapeARot.getRow( 1 ).z(),
			shapeARot.getRow( 2 ).x(), shapeARot.getRow( 2 ).y(), shapeARot.getRow( 2 ).z(),
			shapeBRot.getRow( 0 ).x(), shapeBRot.getRow( 0 ).y(), shapeBRot.getRow( 0 ).z(),
			shapeBRot.getRow( 1 ).x(), shapeBRot.getRow( 1 ).y(), shapeBRot.getRow( 1 ).z(),
			shapeBRot.getRow( 2 ).x(), shapeBRot.getRow( 2 ).y(), shapeBRot.getRow( 2 ).z());
		fputs( str, fp );

		fclose( fp );
#endif //DEBUG_ME
	}
	else if ( key == 'P' || key =='p' )
	{
		g_pauseAnim = !g_pauseAnim;
	}

	defaultKeyboard(key, x, y);
}

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

void	PolyhedralConvexShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
{
	//not yet, return box inertia

	float margin = GetMargin();

	SimdTransform ident;
	ident.setIdentity();
	SimdVector3 aabbMin,aabbMax;
	GetAabb(ident,aabbMin,aabbMax);
	SimdVector3 halfExtents = (aabbMax-aabbMin)*0.5f;

	SimdScalar lx=2.f*(halfExtents.x()+margin);
	SimdScalar ly=2.f*(halfExtents.y()+margin);
	SimdScalar lz=2.f*(halfExtents.z()+margin);
	const SimdScalar x2 = lx*lx;
	const SimdScalar y2 = ly*ly;
	const SimdScalar z2 = lz*lz;
	const SimdScalar scaledmass = mass * 0.08333333f;

	inertia = scaledmass * (SimdVector3(y2+z2,x2+z2,x2+y2));

}

void ReplaceMeFacet::silhouette(int index, const SimdVector3& w, 
			  ReplaceMeEdgeBuffer& edgeBuffer) 
{
    if (!m_obsolete) {
		if (m_closest.dot(w) < m_dist2) {
			edgeBuffer.push_back(ReplaceMeEdge(this, index));
		}	
	else {
	    m_obsolete = true; // Facet is visible 
	    int next = incMod3(index);
	    m_adjFacets[next]->silhouette(m_adjEdges[next], w, edgeBuffer);
	    next = incMod3(next);
	    m_adjFacets[next]->silhouette(m_adjEdges[next], w, edgeBuffer);
	}
    }
}

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

void	CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
{
	RigidBody* body = ctrl->GetRigidBody();

	//this m_userPointer is just used for triggers, see CallbackTriggers
	body->m_userPointer = ctrl;

	body->setGravity( m_gravity );
	m_controllers.push_back(ctrl);

	m_collisionWorld->AddCollisionObject(body,ctrl->GetCollisionFilterGroup(),ctrl->GetCollisionFilterMask());

	assert(body->m_broadphaseHandle);

	BroadphaseInterface* scene =  GetBroadphase();


	CollisionShape* shapeinterface = ctrl->GetCollisionShape();

	assert(shapeinterface);

	const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
	
	body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();

	SimdPoint3 minAabb,maxAabb;

	shapeinterface->GetAabb(t,minAabb,maxAabb);

	float timeStep = 0.02f;


	//extent it with the motion

	SimdVector3 linMotion = body->getLinearVelocity()*timeStep;

	float maxAabbx = maxAabb.getX();
	float maxAabby = maxAabb.getY();
	float maxAabbz = maxAabb.getZ();
	float minAabbx = minAabb.getX();
	float minAabby = minAabb.getY();
	float minAabbz = minAabb.getZ();

	if (linMotion.x() > 0.f)
		maxAabbx += linMotion.x(); 
	else
		minAabbx += linMotion.x();
	if (linMotion.y() > 0.f)
		maxAabby += linMotion.y(); 
	else
		minAabby += linMotion.y();
	if (linMotion.z() > 0.f)
		maxAabbz += linMotion.z(); 
	else
		minAabbz += linMotion.z();


	minAabb = SimdVector3(minAabbx,minAabby,minAabbz);
	maxAabb = SimdVector3(maxAabbx,maxAabby,maxAabbz);




}

void	CcdPhysicsEnvironment::UpdateAabbs(float	timeStep)
{
	std::vector<CcdPhysicsController*>::iterator i;
	BroadphaseInterface* scene =  GetBroadphase();

	//
			// update aabbs, only for moving objects (!)
			//
			for (i=m_controllers.begin();
				!(i==m_controllers.end()); i++)
			{
				CcdPhysicsController* ctrl = (*i);
				RigidBody* body = ctrl->GetRigidBody();


				SimdPoint3 minAabb,maxAabb;
				CollisionShape* shapeinterface = ctrl->GetCollisionShape();



				shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(),
					body->getLinearVelocity(),
					//body->getAngularVelocity(),
					SimdVector3(0.f,0.f,0.f),//no angular effect for now //body->getAngularVelocity(),
					timeStep,minAabb,maxAabb);


				SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
				minAabb -= manifoldExtraExtents;
				maxAabb += manifoldExtraExtents;

				BroadphaseProxy* bp = body->m_broadphaseHandle;
				if (bp)
				{

					SimdVector3 color (1,1,0);

					if (m_debugDrawer)
					{	
						//draw aabb
						switch (body->GetActivationState())
						{
						case ISLAND_SLEEPING:
							{
								color.setValue(1,1,1);
								break;
							}
						case WANTS_DEACTIVATION:
							{
								color.setValue(0,0,1);
								break;
							}
						case ACTIVE_TAG:
							{
								break;
							}
						case DISABLE_DEACTIVATION:
							{
								color.setValue(1,0,1);
							};

						};

						if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
						{
							DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
						}
					}

			
					if ( (maxAabb-minAabb).length2() < 1e12f)
					{
						scene->SetAabb(bp,minAabb,maxAabb);
					} else
					{
						//something went wrong, investigate
						//removeCcdPhysicsController(ctrl);
						body->SetActivationState(DISABLE_SIMULATION);

						static bool reportMe = true;
						if (reportMe)
						{
							reportMe = false;
							printf("Overflow in AABB, object removed from simulation \n");
							printf("If you can reproduce this, please email [email protected]\n");
							printf("Please include above information, your Platform, version of OS.\n");
							printf("Thanks.\n");
						}
						
					}

				}
			}
}