C++ PxVec3::magnitude方法代码示例

// TODO: I'm sure there is a more efficient approach.
static void TransformNormalToShapeSpace(const PxMeshScale& meshScale, const PxVec3& nIn, PxVec3& nOut)
{
	// Uniform scale makes this unnecessary
	if (meshScale.scale.x == meshScale.scale.y &&
		meshScale.scale.x == meshScale.scale.z)
	{
		nOut = nIn;
		return;
	}

	PxMat33 R(meshScale.rotation);
	PxMat33 vertex2ShapeSkew = R.getTranspose();
	PxMat33 diagonal = PxMat33::createDiagonal(meshScale.scale);
	vertex2ShapeSkew = vertex2ShapeSkew * diagonal;
	vertex2ShapeSkew = vertex2ShapeSkew * R;

	PxMat33 shape2VertexSkew = vertex2ShapeSkew.getInverse();
	const PxVec3 tmp = shape2VertexSkew.transformTranspose(nIn);
	const PxReal Denom = 1.0f / tmp.magnitude();
	nOut = tmp * Denom;
}

void collideWithSphereNonContinuous(PxsParticleCollData& collData, const PxVec3& pos, const PxReal& radius,
									const PxReal& proxRadius)
{
	if(collData.localFlags & PXS_FLUID_COLL_FLAG_CC)
		return;		// Only apply discrete and proximity collisions if no continuous collisions was detected so far (for any colliding shape)

	PxReal dist = pos.magnitude();
	collData.localSurfaceNormal = pos;
	if(dist < (radius + proxRadius))
	{
		if (dist != 0.0f)
			collData.localSurfaceNormal *= (1.0f / dist);
		else
			collData.localSurfaceNormal = PxVec3(0);

		// Push particle to surface such that the distance to the surface is equal to the collision radius
		collData.localSurfacePos = collData.localSurfaceNormal * (radius + collData.restOffset);
		collData.localFlags |= PXS_FLUID_COLL_FLAG_L_PROX;

		if(dist < (radius + collData.restOffset))
			collData.localFlags |= PXS_FLUID_COLL_FLAG_L_DC;
	}
}

// PT: ray-capsule intersection code, originally from the old Magic Software library.
PxU32 Gu::intersectRayCapsuleInternal(const PxVec3& origin, const PxVec3& dir, const PxVec3& p0, const PxVec3& p1, float radius, PxReal s[2])
{
	// set up quadratic Q(t) = a*t^2 + 2*b*t + c
	PxVec3 kW = p1 - p0;
	const float fWLength = kW.magnitude();
	if(fWLength!=0.0f)
		kW /= fWLength;

	// PT: if the capsule is in fact a sphere, switch back to dedicated sphere code.
	// This is not just an optimization, the rest of the code fails otherwise.
	if(fWLength<=1e-6f)
	{
		const float d0 = (origin - p0).magnitudeSquared();
		const float d1 = (origin - p1).magnitudeSquared();
		const float approxLength = (PxMax(d0, d1) + radius)*2.0f;
		return PxU32(Gu::intersectRaySphere(origin, dir, approxLength, p0, radius, s[0]));
	}

	// generate orthonormal basis
	PxVec3 kU(0.0f);

	if (fWLength > 0.0f)
	{
		PxReal fInvLength;
		if ( PxAbs(kW.x) >= PxAbs(kW.y) )
		{
			// W.x or W.z is the largest magnitude component, swap them
			fInvLength = PxRecipSqrt(kW.x*kW.x + kW.z*kW.z);
			kU.x = -kW.z*fInvLength;
			kU.y = 0.0f;
			kU.z = kW.x*fInvLength;
		}
		else
		{
			// W.y or W.z is the largest magnitude component, swap them
			fInvLength = PxRecipSqrt(kW.y*kW.y + kW.z*kW.z);
			kU.x = 0.0f;
			kU.y = kW.z*fInvLength;
			kU.z = -kW.y*fInvLength;
		}
	}

	PxVec3 kV = kW.cross(kU);
	kV.normalize();	// PT: fixed november, 24, 2004. This is a bug in Magic.

	// compute intersection

	PxVec3 kD(kU.dot(dir), kV.dot(dir), kW.dot(dir));
	const float fDLength = kD.magnitude();
	const float fInvDLength = fDLength!=0.0f ? 1.0f/fDLength : 0.0f;
	kD *= fInvDLength;

	const PxVec3 kDiff = origin - p0;
	const PxVec3 kP(kU.dot(kDiff), kV.dot(kDiff), kW.dot(kDiff));
	const PxReal fRadiusSqr = radius*radius;

	// Is the velocity parallel to the capsule direction? (or zero)
	if ( PxAbs(kD.z) >= 1.0f - PX_EPS_REAL || fDLength < PX_EPS_REAL )
	{
		const float fAxisDir = dir.dot(kW);

		const PxReal fDiscr = fRadiusSqr - kP.x*kP.x - kP.y*kP.y;
		if ( fAxisDir < 0 && fDiscr >= 0.0f )
		{
			// Velocity anti-parallel to the capsule direction
			const PxReal fRoot = PxSqrt(fDiscr);
			s[0] = (kP.z + fRoot)*fInvDLength;
			s[1] = -(fWLength - kP.z + fRoot)*fInvDLength;
			return 2;
		}
		else if ( fAxisDir > 0  && fDiscr >= 0.0f )
		{
			// Velocity parallel to the capsule direction
			const PxReal fRoot = PxSqrt(fDiscr);
			s[0] = -(kP.z + fRoot)*fInvDLength;
			s[1] = (fWLength - kP.z + fRoot)*fInvDLength;
			return 2;
		}
		else
		{
			// sphere heading wrong direction, or no velocity at all
			return 0;
		}   
	}

	// test intersection with infinite cylinder
	PxReal fA = kD.x*kD.x + kD.y*kD.y;
	PxReal fB = kP.x*kD.x + kP.y*kD.y;
	PxReal fC = kP.x*kP.x + kP.y*kP.y - fRadiusSqr;
	PxReal fDiscr = fB*fB - fA*fC;
	if ( fDiscr < 0.0f )
	{
		// line does not intersect infinite cylinder
		return 0;
	}

	PxU32 iQuantity = 0;

	if ( fDiscr > 0.0f )
//.........这里部分代码省略.........

bool Character::move(PxReal speed, bool jump, bool active )
{
	if (mCurrentMotion == NULL)
		return false;

	if (mBlendCounter > 0)
		mBlendCounter--;

	if (mTargetMotion && (mBlendCounter == 0))
	{
		mBlendCounter = 0;
		mCurrentMotion = mTargetMotion;
		mFrameTime = 0.0f;
		mTargetMotion = NULL;
	}

	PxU32 nbFrames = mCurrentMotion->mNbFrames;
	PxReal distance = mCurrentMotion->mDistance;

	PxReal frameDelta = 0.0f;
	const PxReal angleLimitPerFrame = 3.0f;

	if (jump)
	{
		frameDelta = 1.0f;
	}
	else if (active && (mBlendCounter == 0))
	{
		// compute target orientation
		PxVec3 dir = mTargetPosition - mCharacterPose.p;
		dir.y = 0.0f;
		PxReal curDistance = dir.magnitude();

		if (curDistance > 0.01f)
			faceToward(dir, angleLimitPerFrame);
	
		frameDelta = speed * PxReal(nbFrames) * (curDistance / distance);
	}
	else
	{
		frameDelta = 0;
	}

	mCharacterPose.p = mTargetPosition;

	mFrameTime += frameDelta;
	PxU32 frameNo = PxU32(mFrameTime);

	if (frameNo >= nbFrames)
	{
		if (jump)
			mFrameTime = PxReal(nbFrames) - 1.0f;
		else
			mFrameTime = 0.0f;
	}

	// compute pose of all the bones at current frame (results are used by both getFramePose and Skin)
	computeFramePose();

	return true;
}

//.........这里部分代码省略.........
		vertexHeap.clear();
		float* vertexDistance = &vertexDistanceBuffer[0] + (i * mNumParticles);
		PxU32* vertexParent = &vertexParentBuffer[0] + (i * mNumParticles);

		// initialize parent and distance
		for (PxU32 j = 0; j < mNumParticles; ++j)
		{
			vertexParent[j] = j;
			vertexDistance[j] = PX_MAX_F32;
		}

		// put all the attached vertices in this island to heap
		const PxU32 beginIsland = islandFirst[i];
		const PxU32 endIsland = islandFirst[i+1];
		for (PxU32 j = beginIsland; j < endIsland; j++)
		{
			PxU32 vj = islandIndices[j];
			vertexDistance[vj] = 0.0f;
			vertexHeap.pushBack(VertexDistanceCount((int)vj, 0.0f, 0));
		}

		// no attached vertices in this island (error?)
		PX_ASSERT(vertexHeap.empty() == false);
		if (vertexHeap.empty())
			continue;

		// while heap is not empty
		while (!vertexHeap.empty())
		{
			// pop vi from heap
			VertexDistanceCount vi = popHeap(vertexHeap);

			// obsolete entry ( we already found better distance)
			if (vi.distance > vertexDistance[vi.vertNr])
				continue;

			// for each adjacent vj that's not visited
			const PxI32 begin = (PxI32)valency[(PxU32)vi.vertNr];
			const PxI32 end = (PxI32)valency[PxU32(vi.vertNr + 1)];
			for (PxI32 j = begin; j < end; ++j)
			{
				const PxI32 vj = (PxI32)neighbors[(PxU32)j];
				PxVec3 edge = mVertices[(PxU32)vj] - mVertices[(PxU32)vi.vertNr];
				const PxF32 edgeLength = edge.magnitude();
				float newDistance = vi.distance + edgeLength;

				if (newDistance < vertexDistance[vj])
				{
					vertexDistance[vj] = newDistance;
					vertexParent[vj] = vertexParent[vi.vertNr];
	
					pushHeap(vertexHeap, VertexDistanceCount(vj, newDistance, 0));
				}
			}
		}
	}

	const PxU32 maxTethersPerParticle = 4; // max tethers
	const PxU32 nbTethersPerParticle = (islandCnt > maxTethersPerParticle) ? maxTethersPerParticle : islandCnt;

	PxU32 nbTethers = nbTethersPerParticle * mNumParticles;
	mTetherAnchors.resize(nbTethers);
	mTetherLengths.resize(nbTethers);

	// now process the parent and distance and add to fibers
	for (PxU32 i = 0; i < mNumParticles; i++)
	{
		// we use the heap to sort out N-closest island
		vertexHeap.clear();
		for (PxU32 j = 0; j < islandCnt; j++)
		{
			int parent = (int)vertexParentBuffer[j * mNumParticles + i];
			float edgeDistance = vertexDistanceBuffer[j * mNumParticles + i];
			pushHeap(vertexHeap, VertexDistanceCount(parent, edgeDistance, 0));
		}

		// take out N-closest island from the heap
		for (PxU32 j = 0; j < nbTethersPerParticle; j++)
		{
			VertexDistanceCount vi = popHeap(vertexHeap);
			PxU32 parent = (PxU32)vi.vertNr;
			float distance = 0.0f;
		
			if (parent != i) 
			{
				float euclideanDistance = (mVertices[i] - mVertices[parent]).magnitude();
				float dijkstraDistance = vi.distance;
				int errorCode = 0;
				float geodesicDistance = computeGeodesicDistance(i,parent, errorCode);
				if (errorCode < 0)
					geodesicDistance = dijkstraDistance;
				distance = PxMax(euclideanDistance, geodesicDistance);
			}

			PxU32 tetherLoc = j * mNumParticles + i;
			mTetherAnchors[ tetherLoc ] = parent;
			mTetherLengths[ tetherLoc ] = distance;
		}
	}
}

bool doRaycastCCD(PxShape* shape, PxTransform& newPose, PxVec3& newShapeCenter, const PxVec3& ccdWitness, const PxVec3& ccdWitnessOffset)
{
	PxRigidDynamic* dyna = canDoCCD(shape);
	if(!dyna)
		return true;

	bool updateCCDWitness = true;

	const PxVec3 offset = newPose.p - newShapeCenter;
//printf("CCD0: %f | %f | %f\n", newShapeCenter.x, newShapeCenter.y, newShapeCenter.z);
	const PxVec3& origin = ccdWitness;
//			const PxVec3& dest = newPose.p;
	const PxVec3& dest = newShapeCenter;

	PxVec3 dir = dest - origin;
	const PxReal length = dir.magnitude();
	if(length!=0.0f)
	{
		dir /= length;

		// Compute internal radius
//		PxVec3 localCenter;
		const PxReal internalRadius = computeInternalRadius(shape, dir, /*localCenter,*/ ccdWitnessOffset);

		// Compute distance to impact
		PxRaycastHit hit;
//		if(internalRadius!=0.0f && CCDRaycast(shape->getActor().getActiveScene(), origin + localCenter, dir, length, hit))
		if(internalRadius!=0.0f && CCDRaycast(shape->getActor().getScene(), origin, dir, length, hit))
		{
#ifdef RAYCAST_CCD_PRINT_DEBUG
			static int count=0;
			printf("CCD hit %d\n", count++);
#endif
			updateCCDWitness = false;
			const PxReal radiusLimit = internalRadius * 0.75f;
			if(hit.distance>radiusLimit)
			{
//				newPose.p = origin + dir * (hit.distance - radiusLimit);
				newShapeCenter = origin + dir * (hit.distance - radiusLimit);
#ifdef RAYCAST_CCD_PRINT_DEBUG
				printf("  Path0: %f | %f\n", hit.distance, radiusLimit);
#endif
			}
			else
			{
//				newPose.p = origin;
				newShapeCenter = origin;
//				newShapeCenter = origin + hit.normal * (radiusLimit - hit.distance);
#ifdef RAYCAST_CCD_PRINT_DEBUG
				printf("  Path1: %f\n", hit.distance);
#endif
			}

			{
				newPose.p = offset + newShapeCenter;
//newPose.p.y += 10.0f;
//printf("%f | %f | %f\n", newPose.p.x, newPose.p.y, newPose.p.z);

//				dyna->setGlobalPose(newPose);

				// newPose = actorGlobalPose * shapeLocalPose
				// newPose * inverse(shapeLocalPose) = actorGlobalPose

				const PxTransform shapeLocalPose = shape->getLocalPose();
				const PxTransform inverseShapeLocalPose = shapeLocalPose.getInverse();
				PxTransform newGlobalPose = newPose * inverseShapeLocalPose;
				dyna->setGlobalPose(newGlobalPose);
//dyna->setGlobalPose(newPose);
//printf("%f | %f | %f\n", newGlobalPose.p.x, newGlobalPose.p.y, newGlobalPose.p.z);
//printf("%f | %f | %f\n", shapeLocalPose.p.x, shapeLocalPose.p.y, shapeLocalPose.p.z);

/*PX_INLINE PxTransform PxShapeExt::getGlobalPose(const PxShape& shape)
{
PxRigidActor& ra = shape.getActor();

return ra.getGlobalPose() * shape.getLocalPose();
}*/
const PxVec3 testShapeCenter = getShapeCenter(shape, ccdWitnessOffset);
float d = (testShapeCenter - newShapeCenter).magnitude();
//printf("%f\n", d);
//printf("CCD1: %f | %f | %f\n", testShapeCenter.x, testShapeCenter.y, testShapeCenter.z);

//dyna->clearForce(PxForceMode::eFORCE);
//dyna->clearForce(PxForceMode::eIMPULSE);
//dyna->setLinearVelocity(PxVec3(0));	// PT: this helps the CCT but stops small objects dead, which doesn't look great

			}
		}
	}
	return updateCCDWitness;
}

/**
 @brief 
 @date 2014-01-05
*/
void RendererCompositionShape::FindMostCloseFace( 
	void *positions0, PxU32 positionStride0, void *normals0, PxU32 normalStride0,
	PxU16 *indices0, PxU32 idx0Size,
	void *positions1, PxU32 positionStride1, void *normals1, PxU32 normalStride1,
	PxU16 *indices1, PxU32 idx1Size,
	OUT std::pair<int,int> &closeFace0, OUT std::pair<int,int> &closeFace1,
	OUT set<PxU16> &vtx0, OUT set<PxU16> &vtx1 )
{
	int foundCount = 0;
	set<int> checkV0, checkV1;

	while (foundCount < 2)
	{
		float minLen = 100000.f;
		int minFaceIdx0 = -1;
		int minFaceIdx1 = -1;

		// find most close face
		for (PxU8 i=0; i<idx0Size; i+=3)
		{
			if (checkV0.find(i) != checkV0.end())
				continue; // already exist face index

			PxVec3 center0;
			PxVec3 center0Normal;
			{
				const PxU16 vidx0 = indices0[ i];
				const PxU16 vidx1 = indices0[ i+1];
				const PxU16 vidx2 = indices0[ i+2];

				PxVec3 &p0 = *(PxVec3*)(((PxU8*)positions0) + (positionStride0 * vidx0));
				PxVec3 &p1 = *(PxVec3*)(((PxU8*)positions0) + (positionStride0 * vidx1));
				PxVec3 &p2 = *(PxVec3*)(((PxU8*)positions0) + (positionStride0 * vidx2));

				PxVec3 &n0 = *(PxVec3*)(((PxU8*)normals0) + (normalStride0 * vidx0));
				PxVec3 &n1 = *(PxVec3*)(((PxU8*)normals0) + (normalStride0 * vidx1));
				PxVec3 &n2 = *(PxVec3*)(((PxU8*)normals0) + (normalStride0 * vidx2));

				center0 = p0 + p1 + p2;
				center0 /= 3.f;

				center0Normal = n0;
			}

			for (PxU8 k=0; k<idx1Size; k+=3)
			{
				if (checkV1.find(k) != checkV1.end())
					continue; // already exist face index

				PxVec3 center1;
				PxVec3 center1Normal;
				{
					const PxU16 vidx0 = indices1[ k];
					const PxU16 vidx1 = indices1[ k+1];
					const PxU16 vidx2 = indices1[ k+2];

					PxVec3 &p0 = *(PxVec3*)(((PxU8*)positions1) + (positionStride1 * vidx0)) + PxVec3(1,0,0);
					PxVec3 &p1 = *(PxVec3*)(((PxU8*)positions1) + (positionStride1 * vidx1)) + PxVec3(1,0,0);
					PxVec3 &p2 = *(PxVec3*)(((PxU8*)positions1) + (positionStride1 * vidx2)) + PxVec3(1,0,0);

					PxVec3 &n0 = *(PxVec3*)(((PxU8*)normals1) + (normalStride1 * vidx0));
					PxVec3 &n1 = *(PxVec3*)(((PxU8*)normals1) + (normalStride1 * vidx1));
					PxVec3 &n2 = *(PxVec3*)(((PxU8*)normals1) + (normalStride1 * vidx2));

					center1 = p0 + p1 + p2;
					center1 /= 3.f;

					center1Normal = n0;
				}

				PxVec3 len = center0 - center1;
				if ((minLen > len.magnitude()) && (center0Normal.dot(center1Normal) < 0) )
				{
					minFaceIdx0 = i;
					minFaceIdx1 = k;
					minLen = len.magnitude();	
				}
			}
		}

		checkV0.insert(minFaceIdx0);
		checkV1.insert(minFaceIdx1);

		if (foundCount == 0)
		{
			closeFace0 = std::pair<int,int>(minFaceIdx0, minFaceIdx1);
		}
		else
		{
			closeFace1 = std::pair<int,int>(minFaceIdx0, minFaceIdx1);
		}

		++foundCount;
	}


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

/**
 @brief Mouse Move
 @date 2014-03-04
*/
void COrientationEditController::MouseMove(physx::PxU32 x, physx::PxU32 y)
{
	switch (m_editMode)
	{
	case MODE_NONE: 
		break;

	case MODE_POSITION:
		{
			RET(!m_selectNode);

			using namespace genotype_parser;
			//const PxTransform tm = GetJointTransformAccumulate(m_rootNode, m_selectNode);
			const SConnection *joint = m_rootNode->GetJoint(m_selectNode);
			BRK(!joint);
	
			const PxVec3 initialPos = utility::Vec3toPxVec3(joint->pos);
			const PxVec3 dimension = utility::Vec3toPxVec3(m_selectNode->m_expr->dimension);
			const float len = initialPos.magnitude() + dimension.magnitude();

			PxVec3 orig, dir, pickOrig;
			m_sample.GetPicking()->computeCameraRay(orig, dir, pickOrig, x, y);
			PxVec3 pos = pickOrig + (dir*3);
			 
			PxVec3 v = pos - m_rootNode->GetPos();
			if (v.magnitude() > len)
			{
				v.normalize();
				pos = m_rootNode->GetPos() + (v*len);
			}

			m_selectNode->SetWorldTransform(PxTransform(pos));
			//m_selectNode->m_renderNode->setTransform(PxTransform(pos));
		}
		break;

	case MODE_PICKUP:
		PickupNodes(m_sample, m_nodes, x, y, true);
		break;

	case MODE_ROTATION:
		{
			PxVec3 v0;
			const Int2 pos(x,y);
			if (pos == m_ptOrig)
				break;

			//const Int2 diff = pos - m_ptOrig;
			{
				PxVec3 orig0, dir0, pickOrig0;
				m_sample.GetPicking()->computeCameraRay(orig0, dir0, pickOrig0, x, y);
				PxVec3 orig1, dir1, pickOrig1;
				m_sample.GetPicking()->computeCameraRay(orig1, dir1, pickOrig1, m_ptOrig.x, m_ptOrig.y);

				PxVec3 v1 = pickOrig0 - pickOrig1;
				v1.normalize();

				v0 = dir0.cross(v1);
				v0.normalize();
			}
			m_ptOrig = pos;


			PxTransform tm = m_selectNode->GetLocalTransform();
			PxTransform m = PxTransform(PxQuat(-0.03f, v0)) * tm;
			m_selectNode->SetLocalTransform(m);
			m_selectNode->UpdateTransform();

			//PxQuat q0(diff.x*0.05f, PxVec3(0,1,0));
			//PxQuat q1(-diff.y*0.05f, PxVec3(1,0,0));
			//PxTransform m = tm * PxTransform(q1) * PxTransform(q0);
			//m_selectNode->m_renderNode->setTransform(m);
		}
		break;
	}

	if (m_cursor)
		m_cursor->MouseMove(x,y);
}

	PX_FORCE_INLINE PxU32 collideWithMeshTriangle(PxVec3& surfaceNormal, PxVec3& surfacePos,
								  PxVec3& proxSurfaceNormal, PxVec3& proxSurfacePos,
								  PxReal& ccTime, PxReal& distOldToSurface,
								  const PxVec3& oldPos, const PxVec3& newPos,
								  const PxVec3& origin, const PxVec3& e0,
								  const PxVec3& e1, bool hasCC,
								  const PxReal& collRadius, const PxReal& proxRadius)
	{
		PxU32 flags = 0;

		PxReal collisionRadius2 = collRadius * collRadius;
		PxReal proximityRadius2 = proxRadius * proxRadius;

		PxVec3 motion = newPos - oldPos;

		// dc and proximity tests
		PxVec3 tmpV = origin - newPos;

		PxReal a = e0.dot(e0);
		PxReal b = e0.dot(e1);
		PxReal c = e1.dot(e1);
		PxReal d = e0.dot(tmpV);
		PxReal e = e1.dot(tmpV);
		PxVec3 coords;
		coords.x = b*e - c*d;	// s * det
		coords.y = b*d - a*e;	// t * det
		coords.z = a*c - b*b;	// det

		bool insideCase = false;
		PxVec3 clampedCoords(PxVec3(0));
		if (coords.x <= 0.0f) 
		{
			c = PxMax(c, FLT_MIN);
			clampedCoords.y = -e/c;
		}
		else if (coords.y <= 0.0f) 
		{
			a = PxMax(a, FLT_MIN);
			clampedCoords.x = -d/a;
		}
		else if (coords.x + coords.y > coords.z) 
		{
			PxReal denominator = a + c - b - b;
			PxReal numerator   = c + e - b - d;
			denominator = PxMax(denominator, FLT_MIN);
			clampedCoords.x = numerator / denominator;
			clampedCoords.y = 1.0f - clampedCoords.x;
		}
		else // all inside 
		{	
			PxReal tmpF = PxMax(coords.z, FLT_MIN);
			tmpF = 1.0f / tmpF;
			clampedCoords.x = coords.x * tmpF;
			clampedCoords.y = coords.y * tmpF;
			insideCase = true;
		}
		clampedCoords.x = PxMax(clampedCoords.x, 0.0f);
		clampedCoords.y = PxMax(clampedCoords.y, 0.0f);
		clampedCoords.x = PxMin(clampedCoords.x, 1.0f);
		clampedCoords.y = PxMin(clampedCoords.y, 1.0f);

		// Closest point to particle inside triangle
		PxVec3 closest = origin + e0 * clampedCoords.x + e1 * clampedCoords.y;

		PxVec3 triangleOffset = newPos - closest;
		PxReal triangleDistance2 = triangleOffset.magnitudeSquared();

		PxVec3 triangleNormal = e0.cross(e1);
		PxReal e0e1Span = triangleNormal.magnitude();
		
		bool isInFront = triangleOffset.dot(triangleNormal) > 0.0f;

		// MS: Possible optimzation
		/*
		if (isInFront && (triangleDistance2 >= proximityRadius2))
			return flags;
		*/

		bool isInProximity = insideCase && (triangleDistance2 < proximityRadius2) && isInFront;
		bool isInDiscrete = (triangleDistance2 < collisionRadius2) && isInFront;

		if (!hasCC)
		{
			// Only apply discrete and proximity collisions if no continuous collisions was detected so far (for any colliding shape)

			if (isInDiscrete)
			{
				if (triangleDistance2 > PXS_FLUID_COLL_TRI_DISTANCE)
				{
					surfaceNormal = triangleOffset * PxRecipSqrt(triangleDistance2);
				}
				else
				{
					surfaceNormal = triangleNormal * (1.0f / e0e1Span);
				}
				surfacePos = closest + (surfaceNormal * collRadius);
				flags |= PXS_FLUID_COLL_FLAG_L_DC;
			}
			
			if (isInProximity)
//.........这里部分代码省略.........

/**
 @brief MouseRButtonUp
 @date 2014-03-04
*/
void COrientationEditController::MouseRButtonUp(physx::PxU32 x, physx::PxU32 y)
{
	switch (m_editMode)
	{
	case MODE_POSITION:
		{
			RET(!m_selectNode);

			using namespace genotype_parser;

			SConnection *joint = m_rootNode->GetJoint(m_selectNode);
			RET(!joint);

			PxVec3 dir = m_selectNode->GetPos() - m_rootNode->GetPos();
			const float len = dir.magnitude();
			dir.normalize();

			PxQuat rotateToXAxis;
			if (boost::iequals(joint->type, "revolute"))
			{
				PxVec3 jointAxis = PxVec3(0,0,1).cross(dir);
				jointAxis.normalize();
				utility::quatRotationArc(rotateToXAxis, PxVec3(1,0,0), jointAxis);
			}
			else
			{
				rotateToXAxis = m_rootNode->GetWorldTransform().q;
			}

			{ // setting parent orientation
				PxReal angle;
				PxVec3 axis;
				rotateToXAxis.toRadiansAndUnitAxis(angle, axis);
				joint->parentOrient.angle = angle;
				joint->parentOrient.dir = utility::PxVec3toVec3(axis);
				 
				printf( "parent angle = %f, dir= %f %f %f\n", angle, axis.x, axis.y, axis.z);
			}
			
			{ // setting select orientation
				PxVec3 pos;
				PxTransform rotTm;
				const PxTransform tm = m_selectNode->GetWorldTransform();
				if (boost::iequals(joint->type, "revolute"))
				{
					rotTm = PxTransform(rotateToXAxis) * PxTransform(tm.q);
					pos = rotTm.rotate(dir*len);
				}
				else
				{
					rotTm = PxTransform(tm.q);
					pos = tm.p;
				}

				PxReal angle;
				PxVec3 axis;
				rotTm.q.toRadiansAndUnitAxis(angle, axis);
				joint->orient.angle = angle;
				joint->orient.dir = utility::PxVec3toVec3(axis);
				joint->pos = utility::PxVec3toVec3(pos);

				printf( "connect angle = %f, dir= %f %f %f\n", angle, axis.x, axis.y, axis.z);
			}

			SelectNode(NULL);
		}
		break;
	}

	if (m_cursor)
		m_cursor->MouseRButtonUp(x,y);
}

void DeformableMesh::calculateInvMasses(Ps::Array<PxReal>& invMasses, PxReal mass) const
{
	PX_ASSERT(mVertexMasses.empty());
	PX_ASSERT(invMasses.size() == mVertexPositions.size());

	// clear output array
	for (PxU32 i = 0; i < invMasses.size(); i++)
		invMasses[i] = 0.0f;
	
	switch (mPrimitiveType)
	{
	case PxDeformablePrimitiveType::eTRIANGLE:
		{
			// first, we compute area associated with each vertex and temporarily store it in invMasses array
			PxReal totalArea = 0.0f;
			const PxU32* i0 = mPrimitives.begin();
			for (PxU32 i = 0; i < mPrimitives.size() / 3; ++i)
			{
				const PxU32* i1 = i0 + 1;
				const PxU32* i2 = i0 + 2;
				PxVec3 d0(mVertexPositions[*i0].x, mVertexPositions[*i0].y, mVertexPositions[*i0].z);
				PxVec3 d1(mVertexPositions[*i1].x, mVertexPositions[*i1].y, mVertexPositions[*i1].z);
				PxVec3 d2(mVertexPositions[*i2].x, mVertexPositions[*i2].y, mVertexPositions[*i2].z);
				d1 = d1 - d0;
				d2 = d2 - d0;
				PxVec3 n = d1.cross(d2);
				PxReal area = 0.5f * n.magnitude();
				invMasses[*i0] += area / 3.0f;
				invMasses[*i1] += area / 3.0f;
				invMasses[*i2] += area / 3.0f;
				totalArea += area;
				i0 += 3;
			}
			// distribute overall mass by associated area
			for (PxU32 i = 0; i < invMasses.size(); i++)
			{
				PxReal area = invMasses[i];
				invMasses[i] = (1.0f / mass) * (totalArea / area);
			}
		}
		break;
	case PxDeformablePrimitiveType::eTETRAHEDRON:
		{
			// first, we compute volume associated with each vertex and temporarily store it in invMasses array
			PxReal totalVolume = 0.0f;
			const PxU32* i0 = mPrimitives.begin();
			for (PxU32 i = 0; i < mPrimitives.size() / 4; i++)
			{
				const PxU32 *i1 = i0 + 1;
				const PxU32 *i2 = i0 + 2;
				const PxU32 *i3 = i0 + 3;
				PxVec3 d0(mVertexPositions[*i0].x, mVertexPositions[*i0].y, mVertexPositions[*i0].z);
				PxVec3 d1(mVertexPositions[*i1].x, mVertexPositions[*i1].y, mVertexPositions[*i1].z);
				PxVec3 d2(mVertexPositions[*i2].x, mVertexPositions[*i2].y, mVertexPositions[*i2].z);
				PxVec3 d3(mVertexPositions[*i3].x, mVertexPositions[*i3].y, mVertexPositions[*i3].z);
				d1 = d1 - d0;
				d2 = d2 - d0;
				d3 = d3 - d0;
				PxVec3 n = d1.cross(d2);
				PxReal volume = n.dot(d3) / 6.0f;
				invMasses[*i0] += volume / 4.0f;
				invMasses[*i1] += volume / 4.0f;
				invMasses[*i2] += volume / 4.0f;
				invMasses[*i3] += volume / 4.0f;
				totalVolume += volume;
				i0 += 4;
			}
			// distribute overall mass by associated volume
			for (PxU32 i = 0; i < invMasses.size(); i++)
			{
				PxReal volume = invMasses[i];
				invMasses[i] = (1.0f / mass) * (totalVolume / volume);
			}

		}
		break;
	default:
		PX_ASSERT(0);
		break;
	}	
}

void DeformableMesh::generateConstraintsFromTetrahedra()
{
	PxU32 edgeIndices[6][2] = { {0,1}, {0,2}, {0,3}, {1,2}, {1,3}, {2,3} };
	PxU32 *tetIndices;

	// - tetrahedra are assumed to be unique (thus no parent code here)

	PxU32 numTetrahedra = mPrimitives.size() / 4;

	DeformableTetraEdge e;
	Ps::Array<DeformableTetraEdge> edges PX_DEBUG_EXP("defoMeshEdges2");
	edges.reserve(numTetrahedra * 6);
	tetIndices = mPrimitives.begin();
	PxU32 i, j;
	for (i = 0; i < numTetrahedra; i++, tetIndices += 4) 
	{
		for(j = 0; j < 6; j++) 
		{
			PxU32 e0 = mVertexToParticleMap[tetIndices[edgeIndices[j][0]]];
			PxU32 e1 = mVertexToParticleMap[tetIndices[edgeIndices[j][1]]];
			e.set(e0, e1, i); edges.pushBack(e);	
		}
	}
	
	quickSortTetraEdges(edges, 0, edges.size()-1);

	mConstraints.resize(numTetrahedra);

	DeformableConstraint constraint;
	DeformableTetraEdge *tetEdges[6];
	tetIndices = mPrimitives.begin();
	
	bool warningIssued = false;
	for (i = 0; i < numTetrahedra; i++, tetIndices += 4) 
	{
		for (j = 0; j < 6; j++) 
		{
			PxU32 e0 = mVertexToParticleMap[tetIndices[edgeIndices[j][0]]];
			PxU32 e1 = mVertexToParticleMap[tetIndices[edgeIndices[j][1]]];
			DeformableTetraEdge goalEdge;
			goalEdge.set(e0, e1, i);
			tetEdges[j] = binarySearchTetraEdge(edges, goalEdge);
		}

		for (j = 0; j < 4; j++)
			constraint.particleId[j] = mVertexToParticleMap[tetIndices[j]];
		
		PxVec3 groundArea = (mWeldedVertices[tetIndices[1]] - mWeldedVertices[tetIndices[0]]).cross(mWeldedVertices[tetIndices[2]] - mWeldedVertices[tetIndices[0]]);
		constraint.restVolume = groundArea.dot(mWeldedVertices[tetIndices[3]] - mWeldedVertices[tetIndices[0]]);
		constraint.flags = 0;

		if (!warningIssued && constraint.restVolume < 0.0f)
		{
			Ps::getFoundation().error(PxErrorCode::eDEBUG_WARNING, __FILE__, __LINE__, "Soft body mesh tetrahedron %d has illegal winding order.", i);
			warningIssued = true;
		}

		for (j = 0; j < 6; j++) 
		{
			PxU32 e0 = mVertexToParticleMap[tetIndices[edgeIndices[j][0]]];
			PxU32 e1 = mVertexToParticleMap[tetIndices[edgeIndices[j][1]]];
			PxVec3 edgeVec = mWeldedVertices[e1] - mWeldedVertices[e0];
			if(tetEdges[j]) 
			{
				PX_ASSERT(tetEdges[j]->tetrahedron == i);
				constraint.restEdgeLengths[j] = edgeVec.magnitude();
			} 
			else 
				constraint.restEdgeLengths[j] = -edgeVec.magnitude();
		}

		mConstraints[i] = constraint;
	}
}

void RenderPhysX3Debug::addConeExt(float r0, float r1, const PxVec3& p0, const PxVec3& p1 , const RendererColor& color, PxU32 renderFlags)
{
	PxVec3 axis = p1 - p0;
	PxReal length = axis.magnitude();
	PxReal rdiff = r0 - r1;
	PxReal sinAngle = rdiff / length;
	PxReal x0 = r0 * sinAngle;
	PxReal x1 = r1 * sinAngle;
	PxVec3 center = 0.5f * (p0 + p1);

	if (length < fabs(rdiff))
		return;

	PxReal r0p = sqrt(r0 * r0 - x0 * x0);
	PxReal r1p = sqrt(r1 * r1 - x1 * x1);

	if (length == 0.0f)
		axis = PxVec3(1,0,0);
	else
		axis.normalize();

	PxVec3 axis1(0.0f);
	axis1[minArgument(abs(axis))] = 1.0f;
	axis1 = axis1.cross(axis);
	axis1.normalize();

	PxVec3 axis2 = axis.cross(axis1);
	axis2.normalize();

	PxMat44 m;
	m.column0 = PxVec4(axis, 0.0f);
	m.column1 = PxVec4(axis1, 0.0f);
	m.column2 = PxVec4(axis2, 0.0f);
	m.column3 = PxVec4(center, 1.0f);

	PxTransform tr(m);

#define NUM_CONE_VERTS 72
	const PxU32 nbVerts = NUM_CONE_VERTS;

	PxVec3 pts0[NUM_CONE_VERTS] ;
	PxVec3 pts1[NUM_CONE_VERTS];
	PxVec3 normals[NUM_CONE_VERTS] ;

	const float step = PxTwoPi / float(nbVerts);
	for (PxU32 i = 0; i < nbVerts; i++)
	{
		const float angle = float(i) * step;
		const float x = cosf(angle);
		const float y = sinf(angle);

		PxVec3 p = PxVec3(0.0f, x, y);

		pts0[i] = tr.transform(r0p * p + PxVec3(-0.5f * length + x0,0,0));
		pts1[i] = tr.transform(r1p * p + PxVec3(0.5f * length + x1, 0, 0));

		normals[i] = tr.q.rotate(p.getNormalized());
		normals[i] = x0 * axis + r0p * normals[i];
		normals[i].normalize();
	}
#undef NUM_CONE_VERTS

	if(renderFlags & RENDER_DEBUG_WIREFRAME)
	{
		for(PxU32 i=0;i<nbVerts;i++)
		{
			addLine(pts1[i], pts0[i], color);
		}
	}

	if(renderFlags & RENDER_DEBUG_SOLID)
	{
		for(PxU32 i=0;i<nbVerts;i++)
		{
			const PxU32 j = (i+1) % nbVerts;
			addTriangle(pts0[i], pts1[j], pts0[j], normals[i], normals[j], normals[j], color);
			addTriangle(pts0[i], pts1[i], pts1[j], normals[i], normals[i], normals[j], color);
		}
	}

}

										const BoxPadded* cullBox)								// Cull data
{
	if(!nbTris)
		return false;

	const bool meshBothSides = hitFlags & PxHitFlag::eMESH_BOTH_SIDES;
	const bool doBackfaceCulling = !isDoubleSided && !meshBothSides;
	const bool anyHit = hitFlags & PxHitFlag::eMESH_ANY;
	const bool testInitialOverlap = !(hitFlags & PxHitFlag::eASSUME_NO_INITIAL_OVERLAP);

	// PT: we can fallback to sphere sweep:
	// - if the capsule is degenerate (i.e. it's a sphere)
	// - if the sweep direction is the same as the capsule axis, in which case we can just sweep the top or bottom sphere

	const PxVec3 extrusionDir = (capsule.p0 - capsule.p1)*0.5f;	// Extrusion dir = capsule segment
	const PxReal halfHeight = extrusionDir.magnitude();
	bool mustExtrude = halfHeight!=0.0f;
	if(!mustExtrude)
	{
		// PT: capsule is a sphere. Switch to sphere path (intersectCapsuleTriangle doesn't work for degenerate capsules)
		return sweepSphereTriangles(nbTris, triangles, capsule.p0, capsule.radius, unitDir, distance, cachedIndex, hit, triNormalOut, isDoubleSided, meshBothSides, anyHit, testInitialOverlap);
	}
	else
	{
		const PxVec3 capsuleAxis = extrusionDir/halfHeight;
		const PxReal colinearity = PxAbs(capsuleAxis.dot(unitDir));
		mustExtrude = (colinearity < (1.0f - COLINEARITY_EPSILON));
	}

	const PxVec3 capsuleCenter = capsule.computeCenter();