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

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;

}

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

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

//.........这里部分代码省略.........
	{
		for (int y=0;y<screenHeight;y++)
		{
			rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
			rayTo += x * dHor;
			rayTo -= y * dVert;
			rayToTrans.setOrigin(rayTo);
			for (int s=0;s<numObjects;s++)
			{
			//	rayFromLocal = transforms[s].inverse()* rayFromTrans;
			//	rayToLocal = transforms[s].inverse()* rayToTrans;

				//choose the continuous collision detection method
				SubsimplexConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver);
				//GjkConvexCast convexCaster(&pointShape,shapePtr[0],&simplexSolver);
				//ContinuousConvexCollision convexCaster(&pointShape,shapePtr[0],&simplexSolver,0);
				
				//	BU_Simplex1to4	ptShape(SimdVector3(0,0,0));//algebraic needs features, doesnt use 'supporting vertex'
				//	BU_CollisionPair convexCaster(&ptShape,shapePtr[0]);


				//reset previous result
				rayResult.m_fraction = 1.f;


				if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,transforms[s],transforms[s],rayResult))
				{
					//float fog = 1.f - 0.1f * rayResult.m_fraction;
					rayResult.m_normal.normalize();

					SimdVector3 worldNormal;
					worldNormal = transforms[s].getBasis() *rayResult.m_normal;

					float light = worldNormal.dot(SimdVector3(0.4f,-1.f,-0.4f));
					if (light < 0.2f)
						light = 0.2f;
					if (light > 1.f)
						light = 1.f;

					rgba = SimdVector4(light,light,light,1.f);
					raytracePicture->SetPixel(x,y,rgba);
				} else
				{
					//clear is already done
					//rgba = SimdVector4(0.f,0.f,0.f,0.f);
					//raytracePicture->SetPixel(x,y,rgba);

				}

				
			}
		}
	}

#define TEST_PRINTF
#ifdef TEST_PRINTF

	
	extern BMF_FontData BMF_font_helv10;
	
	raytracePicture->Printf("CCD RAYTRACER",&BMF_font_helv10);
	char buffer[256];
	sprintf(buffer,"%d RAYS / Frame",screenWidth*screenHeight*numObjects);
	raytracePicture->Printf(buffer,&BMF_font_helv10,0,10);
	

SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
{
	SimdVector3 v;

	SimdScalar upperBoundSqrd = SIMD_INFINITY;
	SimdScalar vSqrd = 0;
#ifdef _DEBUG
	SimdScalar prevVSqrd;
#endif
	SimdScalar delta;

	bool isCloseEnough = false;

	EpaFace* pEpaFace = NULL;

	int nbIterations = 0;
	//int nbMaxIterations = 1000;

	do
	{
		pEpaFace = m_faceEntries.front();
		std::pop_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries );
		m_faceEntries.pop_back();

		if ( !pEpaFace->m_deleted )
		{
#ifdef _DEBUG
			prevVSqrd = vSqrd;
#endif

			vSqrd = pEpaFace->m_vSqrd;

			if ( pEpaFace->m_planeDistance >= 0 )
			{
				v = pEpaFace->m_planeNormal;
			}
			else
			{
				v = pEpaFace->m_v;
			}

#ifdef _DEBUG
			//assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" );
			EPA_DEBUG_ASSERT( ( vSqrd >= prevVSqrd ) ,"vSqrd decreased!" );
#endif //_DEBUG
			EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Zero vector not allowed!" );

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

			SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
			SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );

			SimdPoint3 pWorld = m_transformA( p );
			SimdPoint3 qWorld = m_transformB( q );

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

			// Keep tighest upper bound
			upperBoundSqrd = SimdMin( upperBoundSqrd, delta * delta / vSqrd );
			//assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" );

			isCloseEnough = ( upperBoundSqrd <= ( 1 + 1e-4f ) * vSqrd );

			if ( !isCloseEnough )
			{
				std::list< EpaFace* > newFaces;
				bool expandOk = m_polyhedron.Expand( w, pWorld, qWorld, pEpaFace, newFaces );

				if ( expandOk )
				{
					EPA_DEBUG_ASSERT( !newFaces.empty() ,"EPA polyhedron not expanding ?" );

					bool check    = true;
					bool areEqual = false;

					while ( !newFaces.empty() )
					{
						EpaFace* pNewFace = newFaces.front();
						EPA_DEBUG_ASSERT( !pNewFace->m_deleted ,"New face is deleted!" );

						if ( !pNewFace->m_deleted )
						{
							EPA_DEBUG_ASSERT( ( pNewFace->m_vSqrd > 0 ) ,"Face containing the origin!" );
							EPA_DEBUG_ASSERT( !pNewFace->IsAffinelyDependent() ,"Face is affinely dependent!" );

//#ifdef EPA_POLYHEDRON_USE_PLANES
////							if ( pNewFace->m_planeDistance >= 0 )
////							{
//								// assert( false && "Face's plane distance greater than 0!" );
//#ifdef _DEBUG
////								m_polyhedron._dbgSaveToFile( "epa_beforeFix.dbg" );
//#endif
//								//pNewFace->FixOrder();
//#ifdef _DEBUG
//								//m_polyhedron._dbgSaveToFile( "epa_afterFix.dbg" );
//#endif
////							}
//#endif
//.........这里部分代码省略.........

//.........这里部分代码省略.........

			for ( int i = 0; i < 3; ++i )
			{
				pDiffCoords[ i * nbInitialPoints + minsIndices[ axisIndex ] ] = false;
				pDiffCoords[ i * nbInitialPoints + maxsIndices[ axisIndex ] ] = false;
			}
		}
	}

	if ( nbSuccessfullAxis <= 1 )
	{
		// Degenerate input ?
		assert( false && "nbSuccessfullAxis must be greater than 1!" );
		return false;
	}
	
	delete[] pDiffCoords;
#endif

	//////////////////////////////////////////////////////////////////////////

#ifdef EPA_POLYHEDRON_USE_PLANES
	SimdVector3 v0 = pInitialPoints[ finalPointsIndices[ 1 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
	SimdVector3 v1 = pInitialPoints[ finalPointsIndices[ 2 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
#else
	SimdVector3 v0 = pInitialPoints[ 1 ] - pInitialPoints[ 0 ];
	SimdVector3 v1 = pInitialPoints[ 2 ] - pInitialPoints[ 0 ];
#endif

	SimdVector3 planeNormal = v1.cross( v0 );
	planeNormal.normalize();

#ifdef EPA_POLYHEDRON_USE_PLANES
	SimdScalar planeDistance = pInitialPoints[ finalPointsIndices[ 0 ] ].dot( -planeNormal );
#else
	SimdScalar planeDistance = pInitialPoints[ 0 ].dot( -planeNormal );
#endif

#ifdef EPA_POLYHEDRON_USE_PLANES
	assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 0 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) &&
			"Point should be on plane!" );
	assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 1 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) &&
			"Point should be on plane!" );
	assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 2 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) &&
			"Point should be on plane!" );
#endif

#ifndef EPA_POLYHEDRON_USE_PLANES
	{
		if ( planeDistance > 0 )
		{
			SimdVector3 tmp = pInitialPoints[ 1 ];
			pInitialPoints[ 1 ] = pInitialPoints[ 2 ];
			pInitialPoints[ 2 ] = tmp;

			tmp = pSupportPointsOnA[ 1 ];
			pSupportPointsOnA[ 1 ] = pSupportPointsOnA[ 2 ];
			pSupportPointsOnA[ 2 ] = tmp;

			tmp = pSupportPointsOnB[ 1 ];
			pSupportPointsOnB[ 1 ] = pSupportPointsOnB[ 2 ];
			pSupportPointsOnB[ 2 ] = tmp;
		}
	}

	EpaVertex* pVertexA = CreateVertex( pInitialPoints[ 0 ], pSupportPointsOnA[ 0 ], pSupportPointsOnB[ 0 ] );

bool	VoronoiSimplexSolver::UpdateClosestVectorAndPoints()
{
	
	if (m_needsUpdate)
	{
		m_cachedBC.Reset();

		m_needsUpdate = false;

		switch (numVertices())
		{
		case 0:
				m_cachedValidClosest = false;
				break;
		case 1:
			{
				m_cachedP1 = m_simplexPointsP[0];
				m_cachedP2 = m_simplexPointsQ[0];
				m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
				m_cachedBC.Reset();
				m_cachedBC.SetBarycentricCoordinates(1.f,0.f,0.f,0.f);
				m_cachedValidClosest = m_cachedBC.IsValid();
				break;
			};
		case 2:
			{
			//closest point origin from line segment
					const SimdVector3& from = m_simplexVectorW[0];
					const SimdVector3& to = m_simplexVectorW[1];
					SimdVector3 nearest;

					SimdVector3 p (0.f,0.f,0.f);
					SimdVector3 diff = p - from;
					SimdVector3 v = to - from;
					float t = v.dot(diff);
					
					if (t > 0) {
						float dotVV = v.dot(v);
						if (t < dotVV) {
							t /= dotVV;
							diff -= t*v;
							m_cachedBC.m_usedVertices.usedVertexA = true;
							m_cachedBC.m_usedVertices.usedVertexB = true;
						} else {
							t = 1;
							diff -= v;
							//reduce to 1 point
							m_cachedBC.m_usedVertices.usedVertexB = true;
						}
					} else
					{
						t = 0;
						//reduce to 1 point
						m_cachedBC.m_usedVertices.usedVertexA = true;
					}
					m_cachedBC.SetBarycentricCoordinates(1-t,t);
					nearest = from + t*v;

					m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
					m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
					m_cachedV = m_cachedP1 - m_cachedP2;
					
					ReduceVertices(m_cachedBC.m_usedVertices);

					m_cachedValidClosest = m_cachedBC.IsValid();
					break;
			}
		case 3:
			{
				//closest point origin from triangle
				SimdVector3 p (0.f,0.f,0.f);
				
				const SimdVector3& a = m_simplexVectorW[0];
				const SimdVector3& b = m_simplexVectorW[1];
				const SimdVector3& c = m_simplexVectorW[2];

				ClosestPtPointTriangle(p,a,b,c,m_cachedBC);
				m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
								m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
								m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
								m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];

				m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
					m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
					m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
					m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];

				m_cachedV = m_cachedP1-m_cachedP2;

				ReduceVertices (m_cachedBC.m_usedVertices);
				m_cachedValidClosest =  m_cachedBC.IsValid();

				break;
			}
		case 4:
			{

				
				SimdVector3 p (0.f,0.f,0.f);
				
//.........这里部分代码省略.........

bool	VoronoiSimplexSolver::ClosestPtPointTriangle(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c,SubSimplexClosestResult& result)
{
	result.m_usedVertices.reset();

    // Check if P in vertex region outside A
    SimdVector3 ab = b - a;
    SimdVector3 ac = c - a;
    SimdVector3 ap = p - a;
    float d1 = ab.dot(ap);
    float d2 = ac.dot(ap);
    if (d1 <= 0.0f && d2 <= 0.0f) 
	{
		result.m_closestPointOnSimplex = a;
		result.m_usedVertices.usedVertexA = true;
		result.SetBarycentricCoordinates(1,0,0);
		return true;// a; // barycentric coordinates (1,0,0)
	}

    // Check if P in vertex region outside B
    SimdVector3 bp = p - b;
    float d3 = ab.dot(bp);
    float d4 = ac.dot(bp);
    if (d3 >= 0.0f && d4 <= d3) 
	{
		result.m_closestPointOnSimplex = b;
		result.m_usedVertices.usedVertexB = true;
		result.SetBarycentricCoordinates(0,1,0);

		return true; // b; // barycentric coordinates (0,1,0)
	}
    // Check if P in edge region of AB, if so return projection of P onto AB
    float vc = d1*d4 - d3*d2;
    if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {
        float v = d1 / (d1 - d3);
		result.m_closestPointOnSimplex = a + v * ab;
		result.m_usedVertices.usedVertexA = true;
		result.m_usedVertices.usedVertexB = true;
		result.SetBarycentricCoordinates(1-v,v,0);
		return true;
        //return a + v * ab; // barycentric coordinates (1-v,v,0)
    }

    // Check if P in vertex region outside C
    SimdVector3 cp = p - c;
    float d5 = ab.dot(cp);
    float d6 = ac.dot(cp);
    if (d6 >= 0.0f && d5 <= d6) 
	{
		result.m_closestPointOnSimplex = c;
		result.m_usedVertices.usedVertexC = true;
		result.SetBarycentricCoordinates(0,0,1);
		return true;//c; // barycentric coordinates (0,0,1)
	}

    // Check if P in edge region of AC, if so return projection of P onto AC
    float vb = d5*d2 - d1*d6;
    if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {
        float w = d2 / (d2 - d6);
		result.m_closestPointOnSimplex = a + w * ac;
		result.m_usedVertices.usedVertexA = true;
		result.m_usedVertices.usedVertexC = true;
		result.SetBarycentricCoordinates(1-w,0,w);
		return true;
        //return a + w * ac; // barycentric coordinates (1-w,0,w)
    }

    // Check if P in edge region of BC, if so return projection of P onto BC
    float va = d3*d6 - d5*d4;
    if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {
        float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
		
		result.m_closestPointOnSimplex = b + w * (c - b);
		result.m_usedVertices.usedVertexB = true;
		result.m_usedVertices.usedVertexC = true;
		result.SetBarycentricCoordinates(0,1-w,w);
		return true;		
       // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
    }

    // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
    float denom = 1.0f / (va + vb + vc);
    float v = vb * denom;
    float w = vc * denom;
    
	result.m_closestPointOnSimplex = a + ab * v + ac * w;
	result.m_usedVertices.usedVertexA = true;
	result.m_usedVertices.usedVertexB = true;
	result.m_usedVertices.usedVertexC = true;
	result.SetBarycentricCoordinates(1-v-w,v,w);
	
	return true;
//	return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0f - v - w

}