C++ PxVec3类代码示例

static FVector FindConvexMeshOpposingNormal(const PxLocationHit& PHit, const FVector& TraceDirectionDenorm, const FVector InNormal)
{
	if (IsInvalidFaceIndex(PHit.faceIndex))
	{
		return InNormal;
	}

	PxConvexMeshGeometry PConvexMeshGeom;
	bool bSuccess = PHit.shape->getConvexMeshGeometry(PConvexMeshGeom);
	check(bSuccess);	//should only call this function when we have a convex mesh

	if (PConvexMeshGeom.convexMesh)
	{
		check(PHit.faceIndex < PConvexMeshGeom.convexMesh->getNbPolygons());

		const PxU32 PolyIndex = PHit.faceIndex;
		PxHullPolygon PPoly;
		bool bSuccessData = PConvexMeshGeom.convexMesh->getPolygonData(PolyIndex, PPoly);
		if (bSuccessData)
		{
			// Account for non-uniform scale in local space normal.
			const PxVec3 PPlaneNormal(PPoly.mPlane[0], PPoly.mPlane[1], PPoly.mPlane[2]);
			const PxVec3 PLocalPolyNormal = TransformNormalToShapeSpace(PConvexMeshGeom.scale, PPlaneNormal.getNormalized());

			// Convert to world space
			const PxTransform PShapeWorldPose = PxShapeExt::getGlobalPose(*PHit.shape, *PHit.actor);
			const PxVec3 PWorldPolyNormal = PShapeWorldPose.rotate(PLocalPolyNormal);
			const FVector OutNormal = P2UVector(PWorldPolyNormal);

#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST)
			if (!OutNormal.IsNormalized())
			{
				UE_LOG(LogPhysics, Warning, TEXT("Non-normalized Normal (Hit shape is ConvexMesh): %s (LocalPolyNormal:%s)"), *OutNormal.ToString(), *P2UVector(PLocalPolyNormal).ToString());
				UE_LOG(LogPhysics, Warning, TEXT("WorldTransform \n: %s"), *P2UTransform(PShapeWorldPose).ToString());
			}
#endif
			return OutNormal;
		}
	}

	return InNormal;
}

static PX_FORCE_INLINE Ps::IntBool planeBoxOverlap(const PxVec3& normal, PxReal d, const PxVec3& maxbox)
{
	PxVec3 vmin,vmax;

	if (normal.x>0.0f)
	{
		vmin.x = -maxbox.x;
		vmax.x = maxbox.x;
	}
	else
	{
		vmin.x = maxbox.x;
		vmax.x = -maxbox.x;
	}

	if (normal.y>0.0f)
	{
		vmin.y = -maxbox.y;
		vmax.y = maxbox.y;
	}
	else
	{
		vmin.y = maxbox.y;
		vmax.y = -maxbox.y;
	}

	if (normal.z>0.0f)
	{
		vmin.z = -maxbox.z;
		vmax.z = maxbox.z;
	}
	else
	{
		vmin.z = maxbox.z;
		vmax.z = -maxbox.z;
	}

	if( normal.dot(vmin) + d >  0.0f) return Ps::IntFalse;
	if( normal.dot(vmax) + d >= 0.0f) return Ps::IntTrue;
	return Ps::IntFalse;
}

void Controller::setUpDirectionInternal(const PxVec3& up)
{
    PX_CHECK_MSG(up.isNormalized(), "CCT: up direction must be normalized");

    if(mUserParams.mUpDirection==up)
        return;

//	const PxQuat q = Ps::computeQuatFromNormal(up);
    const PxQuat q = Ps::rotationArc(PxVec3(1.0f, 0.0f, 0.0f), up);

    mUserParams.mQuatFromUp		= q;
    mUserParams.mUpDirection	= up;

    // Update kinematic actor
    if(mKineActor)
    {
        PxTransform pose = mKineActor->getGlobalPose();
        pose.q = q;
        mKineActor->setGlobalPose(pose);
    }
}

static PxQuat rotationArc(const PxVec3& v0, const PxVec3& v1, bool& res)
{
	PxVec3 _v0 = v0;
	PxVec3 _v1 = v1;
	_v0.normalize();
	_v1.normalize();

	float s = sqrtf((1.0f + (v0.dot(v1))) * 2.0f);
	if(s<0.001f)
	{
		res = false;
		return PxQuat::createIdentity();
	}

	PxVec3 p = (_v0.cross(_v1)) / s;
	float w = s * 0.5f;
	PxQuat q(p.x, p.y, p.z, w);
	q.normalize();

	res = true;
	return q;
}

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

/**
 @brief setting control diagramnode
 @date 2014-03-02
*/
void COrientationEditController::SetControlDiagram(CGenotypeNode *node)
{
	RemoveGenotypeTree(m_sample, m_rootNode);
	m_nodes.clear();
	m_rootNode = NULL;

	vector<CGenotypeNode*> nodes;
	m_genotypeController.GetDiagramsLinkto(node, nodes);
	if (nodes.empty())
		return;

	// Create Phenotype Node
	CGenotypeNode *parentNode = nodes[ 0];
	map<const genotype_parser::SExpr*, CGenotypeNode*> symbols;
	const PxTransform identTm = PxTransform::createIdentity();
	m_rootNode = CreatePhenotypeDiagram(identTm,identTm,identTm, parentNode->m_expr, symbols);

	// Camera Setting
	const PxVec3 parentPos = parentNode->GetWorldTransform().p;
	PxVec3 dir = parentPos - node->GetWorldTransform().p;
	dir.normalize();

	PxVec3 left = PxVec3(0,1,0).cross(dir);
	left.normalize();
	PxVec3 camPos = node->GetWorldTransform().p + (left*3.f) + PxVec3(0,2.5f,0);
	PxVec3 camDir = parentPos - camPos;
	camDir.normalize();
	camPos -= (camDir * .5f);

	m_sample.getCamera().lookAt(camPos, parentPos);
	const PxTransform viewTm = m_sample.getCamera().getViewMatrix();
	m_camera->init(viewTm);

	// select Orientation Control node
	if (CGenotypeNode *selectNode = m_rootNode->GetConnectNode(node->m_name))
	{
		selectNode->SetHighLight(true);
		SelectNode(selectNode);
	}
}

bool PxFabricCookerImpl::cook(const PxClothMeshDesc& desc, PxVec3 gravity, bool useGeodesicTether)
{	
	if(!desc.isValid())
	{
		shdfnd::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, 
			"PxFabricCookerImpl::cook: desc.isValid() failed!");
		return false;
	}

	gravity = gravity.getNormalized();

	mNumParticles = desc.points.count;

	// assemble points
	shdfnd::Array<PxVec4> particles;
	particles.reserve(mNumParticles);
	PxStrideIterator<const PxVec3> pIt((const PxVec3*)desc.points.data, desc.points.stride);
	PxStrideIterator<const PxReal> wIt((const PxReal*)desc.invMasses.data, desc.invMasses.stride);
	for(PxU32 i=0; i<mNumParticles; ++i)
		particles.pushBack(PxVec4(*pIt++, wIt.ptr() ? *wIt++ : 1.0f));

	// build adjacent vertex list
	shdfnd::Array<PxU32> valency(mNumParticles+1, 0);
	shdfnd::Array<PxU32> adjacencies;
	if(desc.flags & PxMeshFlag::e16_BIT_INDICES)
		gatherAdjacencies<PxU16>(valency, adjacencies, desc.triangles, desc.quads);
	else
		gatherAdjacencies<PxU32>(valency, adjacencies, desc.triangles, desc.quads);

	// build unique neighbors from adjacencies
	shdfnd::Array<PxU32> mark(valency.size(), 0);
	shdfnd::Array<PxU32> neighbors; neighbors.reserve(adjacencies.size());
	for(PxU32 i=1, j=0; i<valency.size(); ++i)
	{
		for(; j<valency[i]; ++j)
		{
			PxU32 k = adjacencies[j];
			if(mark[k] != i)
			{
				mark[k] = i;
				neighbors.pushBack(k);
			}
		}
		valency[i] = neighbors.size();
	}

	// build map of unique edges and classify
	shdfnd::HashMap<Pair, Edge> edges;
	for(PxU32 i=0; i<mNumParticles; ++i)
	{
		PxReal wi = particles[i].w;
		// iterate all neighbors
		PxU32 jlast = valency[i+1];
		for(PxU32 j=valency[i]; j<jlast; ++j)
		{
			// add 1-ring edge
			PxU32 m = neighbors[j];
			if(wi + particles[m].w > 0.0f)
				edges[Pair(PxMin(i, m), PxMax(i, m))].classify();

			// iterate all neighbors of neighbor
			PxU32 klast = valency[m+1];
			for(PxU32 k=valency[m]; k<klast; ++k)
			{
				PxU32 n = neighbors[k];
				if(n != i && wi + particles[n].w > 0.0f)
				{
					// add 2-ring edge
					edges[Pair(PxMin(i, n), PxMax(i, n))].classify(
						particles[i], particles[m], particles[n]);
				}
			}
		}
	}

	// copy classified edges to constraints array
	// build histogram of constraints per vertex
	shdfnd::Array<Entry> constraints; 	
	constraints.reserve(edges.size());
	valency.resize(0); valency.resize(mNumParticles+1, 0);

	const PxReal sqrtHalf = PxSqrt(0.4f);
	for(shdfnd::HashMap<Pair, Edge>::Iterator eIt = edges.getIterator(); !eIt.done(); ++eIt)
	{
		const Edge& edge = eIt->second;
		const Pair& pair = eIt->first;
		if((edge.mStretching + edge.mBending + edge.mShearing) > 0.0f)
		{	
			PxClothFabricPhaseType::Enum type = PxClothFabricPhaseType::eINVALID;
			if(edge.mBending > PxMax(edge.mStretching, edge.mShearing))
				type = PxClothFabricPhaseType::eBENDING;
			else if(edge.mShearing > PxMax(edge.mStretching, edge.mBending))
				type = PxClothFabricPhaseType::eSHEARING;
			else 
			{
				PxVec4 diff = particles[pair.first]-particles[pair.second];
				PxReal dot = gravity.dot(reinterpret_cast<const PxVec3&>(diff).getNormalized());
				type = fabsf(dot) < sqrtHalf ? PxClothFabricPhaseType::eHORIZONTAL : PxClothFabricPhaseType::eVERTICAL;
			}
			++valency[pair.first];
//.........这里部分代码省略.........

void PxSetJointGlobalFrame(PxJoint& joint, const PxVec3* wsAnchor, const PxVec3* axisIn)
{
	PxRigidActor* actors[2];
	joint.getActors(actors[0], actors[1]);

	PxTransform localPose[2];
	for(PxU32 i=0; i<2; i++)
		localPose[i] = PxTransform::createIdentity();

	// 1) global anchor
	if(wsAnchor)
	{
		//transform anchorPoint to local space
		for(PxU32 i=0; i<2; i++)
			localPose[i].p = actors[i] ? actors[i]->getGlobalPose().transformInv(*wsAnchor) : *wsAnchor;
	}

	// 2) global axis
	if(axisIn)
	{
		PxVec3 localAxis[2], localNormal[2];

		//find 2 orthogonal vectors.
		//gotta do this in world space, if we choose them
		//separately in local space they won't match up in worldspace.
		PxVec3 axisw = *axisIn;
		axisw.normalize();

		PxVec3 normalw, binormalw;
		::normalToTangents(axisw, binormalw, normalw);
		//because axis is supposed to be the Z axis of a frame with the other two being X and Y, we need to negate
		//Y to make the frame right handed. Note that the above call makes a right handed frame if we pass X --> Y,Z, so 
		//it need not be changed.

		for(PxU32 i=0; i<2; i++)
		{
			if(actors[i])
			{
				const PxTransform& m = actors[i]->getGlobalPose();
				PxMat33Legacy mM(m.q);
				localAxis[i]   = mM % axisw;
				localNormal[i] = mM % normalw;
			}
			else
			{
				localAxis[i] = axisw;
				localNormal[i] = normalw;
			}

			PxMat33Legacy rot;
			rot.setColumn(0, localAxis[i]);
			rot.setColumn(1, localNormal[i]);
			rot.setColumn(2, localAxis[i].cross(localNormal[i]));

			localPose[i].q = rot.toQuat();
			localPose[i].q.normalize();
		}
	}

	for(PxU32 i=0; i<2; i++)
		joint.setLocalPose(static_cast<PxJointActorIndex::Enum>( i ), localPose[i]);
}

void GOCCharacterController::ReceiveMessage(Msg &msg)
{
    GOComponent::ReceiveMessage(msg);
    if (msg.typeID == GlobalMessageIDs::PHYSICS_SUBSTEP && !mFreezed)
    {
        GameObjectPtr owner = mOwnerGO.lock();

        float time = msg.params.GetFloat("TIME");
        Ogre::Vector3 finalDir = Ogre::Vector3(0,0,0);
        Ogre::Vector3 userDir = owner->GetGlobalOrientation() * mDirection;

        if (mActor.getPxActor()->isSleeping())
            mActor.getPxActor()->wakeUp();		//Gravity fix

        Ogre::Vector3 playerHalfSize = mDimensions * 0.5f;

        PxTransform transform(OgrePhysX::toPx(owner->GetGlobalPosition()), OgrePhysX::toPx(owner->GetGlobalOrientation()));
        transform.p.y += playerHalfSize.y;

        //sweep filter data - only check against shapes with filter data DYNAMICBODY or STATICBODY
        PxSceneQueryFilterData filterData;
        filterData.data.word0 = CollisionGroups::DYNAMICBODY|CollisionGroups::STATICBODY;
        filterData.flags = PxSceneQueryFilterFlag::eDYNAMIC|PxSceneQueryFilterFlag::eSTATIC;

        //touches ground check
        PxBoxGeometry playerGeometry(playerHalfSize.x, playerHalfSize.y+0.1f, playerHalfSize.z);
        PxShape *outShape;
        mTouchesGround = Main::Instance().GetPhysXScene()->getPxScene()->overlapAny(playerGeometry, transform, outShape, filterData);

        //stair maxStepHeight
        float maxStepHeight = 0.6f;
        PxVec3 currPos = OgrePhysX::Convert::toPx(owner->GetGlobalPosition());

        //feet capsule
        PxBoxGeometry feetVolume(playerHalfSize.x, maxStepHeight*0.5f, playerHalfSize.z);

        //body capsule
        float bodyHeight = mDimensions.y-maxStepHeight;
        PxBoxGeometry bodyVolume(playerHalfSize.x, bodyHeight*0.5f, playerHalfSize.z);

        PxVec3 sweepDirection = OgrePhysX::toPx(userDir);
        float userDirLength = sweepDirection.normalize();

        /*
        We perform two sweeps:
         O    ==> bodyHit?
         |    ==> bodyHit?
        / \   ==> feetHit?

        If there are no hits character can walk in the desired direction.
        If there is a feetHit but no bodyHit player can climb stairs (we add an y-Offset).
        If there is a bodyHit the player can not move.
        */
        PxSweepHit sweepHit;
        transform.p.y = owner->GetGlobalPosition().y + maxStepHeight + bodyHeight*0.5f;
        bool bodyHit = Main::Instance().GetPhysXScene()->getPxScene()->sweepSingle(bodyVolume, transform, sweepDirection, time*userDirLength, PxSceneQueryFlags(), sweepHit, filterData);
        transform.p.y = owner->GetGlobalPosition().y + maxStepHeight*0.5f;
        bool feetHit = Main::Instance().GetPhysXScene()->getPxScene()->sweepSingle(feetVolume, transform, sweepDirection, time*userDirLength, PxSceneQueryFlags(), sweepHit, filterData);

        if (!bodyHit)
        {
            finalDir += userDir;	//add player movement
            if (feetHit)
                finalDir += Ogre::Vector3(0,3,0); //climb stairs
        }

        if (finalDir != Ogre::Vector3(0,0,0))
            mActor.getPxActor()->setGlobalPose(PxTransform(currPos + OgrePhysX::Convert::toPx(finalDir*time)));

        if (mJumping && mTouchesGround && (timeGetTime() - mJumpStartTime > 400))
        {
            mJumping = false;
            Msg jump_response;
            jump_response.typeID = ObjectMessageIDs::END_JUMP;
            BroadcastObjectMessage(jump_response);
        }
    }
    if (msg.typeID == GlobalMessageIDs::PHYSICS_END && !mFreezed)
    {
        SetOwnerPosition(mActor.getGlobalPosition());
        //SetOwnerOrientation(mActor->getGlobalOrientation());
    }


    if (msg.typeID == ObjectMessageIDs::UPDATE_CHARACTER_MOVEMENTSTATE)
    {
        mDirection = Ogre::Vector3(0,0,0);
        int movementFlags = msg.params.GetInt("CharacterMovementState");
        if (movementFlags & CharacterMovement::FORWARD) mDirection.z += 1;
        if (movementFlags & CharacterMovement::BACKWARD) mDirection.z -= 1;
        if (movementFlags & CharacterMovement::LEFT) mDirection.x += 1;
        if (movementFlags & CharacterMovement::RIGHT) mDirection.x -= 1;

        mDirection.normalise();
        mDirection*=(mMovementSpeed*mSpeedFactor);
    }
    if (msg.typeID == ObjectMessageIDs::INPUT_START_JUMP)
    {
        if (mTouchesGround && !mJumping)
        {
//.........这里部分代码省略.........

bool physx::PxMeshQuery::sweep(	const PxVec3& unitDir, const PxReal maxDistance,
								const PxGeometry& geom, const PxTransform& pose,
								PxU32 triangleCount, const PxTriangle* triangles,
								PxSweepHit& sweepHit, PxHitFlags hintFlags_,
								const PxU32* cachedIndex, const PxReal inflation, bool doubleSided)
{
	PX_SIMD_GUARD;
	PX_CHECK_AND_RETURN_VAL(pose.isValid(), "Gu::GeometryQuery::sweep(): pose is not valid.", false);
	PX_CHECK_AND_RETURN_VAL(unitDir.isFinite(), "Gu::GeometryQuery::sweep(): unitDir is not valid.", false);
	PX_CHECK_AND_RETURN_VAL(PxIsFinite(maxDistance), "Gu::GeometryQuery::sweep(): distance is not valid.", false);
	PX_CHECK_AND_RETURN_VAL(maxDistance > 0, "Gu::GeometryQuery::sweep(): sweep distance must be greater than 0.", false);

	const PxReal distance = PxMin(maxDistance, PX_MAX_SWEEP_DISTANCE);

	// PT: the doc says that validity flags are not used, but internally some functions still check them. So
	// to make sure the code works even when no validity flags are passed, we set them all here.
	const PxHitFlags hintFlags = hintFlags_ | PxHitFlag::ePOSITION|PxHitFlag::eNORMAL|PxHitFlag::eDISTANCE;

	switch(geom.getType())
	{
		case PxGeometryType::eSPHERE:
		{
			const PxSphereGeometry& sphereGeom = static_cast<const PxSphereGeometry&>(geom);

			// PT: TODO: technically this capsule with 0.0 half-height is invalid ("isValid" returns false)
			const PxCapsuleGeometry capsuleGeom(sphereGeom.radius, 0.0f);

			return SweepCapsuleTriangles(	triangleCount, triangles, doubleSided, capsuleGeom, pose, unitDir, distance,
											cachedIndex, sweepHit.position, sweepHit.normal, sweepHit.distance, sweepHit.faceIndex, inflation, hintFlags);
		}

		case PxGeometryType::eCAPSULE:
		{
			const PxCapsuleGeometry& capsuleGeom = static_cast<const PxCapsuleGeometry&>(geom);

			return SweepCapsuleTriangles(	triangleCount, triangles, doubleSided, capsuleGeom, pose, unitDir, distance,
											cachedIndex, sweepHit.position, sweepHit.normal, sweepHit.distance, sweepHit.faceIndex, inflation, hintFlags);
		}

		case PxGeometryType::eBOX:
		{
			const PxBoxGeometry& boxGeom = static_cast<const PxBoxGeometry&>(geom);

			if(!PX_IS_SPU && (hintFlags & PxHitFlag::ePRECISE_SWEEP))
			{
				return sweepCCTBoxTriangles(triangleCount, triangles, doubleSided, boxGeom, pose, 
											unitDir, distance, sweepHit.position, sweepHit.normal, sweepHit.distance,
											sweepHit.faceIndex, cachedIndex, inflation, hintFlags);
			}
			else
			{
				return SweepBoxTriangles(	triangleCount, triangles, doubleSided, boxGeom, pose, 
											unitDir, distance, sweepHit.position, sweepHit.normal, sweepHit.distance,
											sweepHit.faceIndex, cachedIndex, inflation, hintFlags);
			}
		}	
		case PxGeometryType::ePLANE:
		case PxGeometryType::eCONVEXMESH:
		case PxGeometryType::eTRIANGLEMESH:
		case PxGeometryType::eHEIGHTFIELD:
		case PxGeometryType::eGEOMETRY_COUNT:
		case PxGeometryType::eINVALID:
		default :
			PX_CHECK_MSG(false, "Gu::GeometryQuery::sweep(): geometry object parameter must be sphere, capsule or box geometry.");
	}
	return false;
}

/**
 @brief vector 4개로 triangle 2개를 그리는 index buffer를 생성한다.
 @date 2014-01-30
*/
void SampleRenderer::GenerateTriangleFrom4Vector2( void *positions, void *normals, void *bones, void *colors, 
	PxU32 stride, PxU32 startVtxIdx, PxU16 *indices, PxU32 startIndexIdx, const PxVec3 &center, 
	const vector<PxU16> &faceIndices, OUT vector<PxU16> &outfaceIndices )
{
	vector<PxU16> triangle0; triangle0.reserve(3);
	vector<PxU16> triangle1; triangle1.reserve(3);
	triangle0.push_back( faceIndices[ 0] );
	triangle0.push_back( faceIndices[ 2] );
	triangle0.push_back( faceIndices[ 3] );

	triangle1.push_back( faceIndices[ 0] );
	triangle1.push_back( faceIndices[ 1] );
	triangle1.push_back( faceIndices[ 3] );

	GenerateTriangleFrom3Vector(positions, normals, stride, center, triangle0, outfaceIndices);
	GenerateTriangleFrom3Vector(positions, normals, stride, center, triangle1, outfaceIndices);

	if (outfaceIndices.size() != 6)
		return; // error occur
	
	for (u_int i=0; i < outfaceIndices.size();)
	{
		const PxVec3 p0 = *(PxVec3*)(((PxU8*)positions) + (stride * outfaceIndices[ i]));
		const PxVec3 p1 = *(PxVec3*)(((PxU8*)positions) + (stride * outfaceIndices[ i+1]));
		const PxVec3 p2 = *(PxVec3*)(((PxU8*)positions) + (stride * outfaceIndices[ i+2]));

		*(PxVec3*)(((PxU8*)positions) + (stride * startVtxIdx)) = p0;
		*(PxVec3*)(((PxU8*)positions) + (stride * (startVtxIdx+1))) = p1;
		*(PxVec3*)(((PxU8*)positions) + (stride * (startVtxIdx+2))) = p2;

		if (bones)
		{
			const PxU32 b0 = *(PxU32*)(((PxU8*)bones) + (stride * outfaceIndices[ i]));
			const PxU32 b1 = *(PxU32*)(((PxU8*)bones) + (stride * outfaceIndices[ i+1]));
			const PxU32 b2 = *(PxU32*)(((PxU8*)bones) + (stride * outfaceIndices[ i+2]));

			*(PxU32*)(((PxU8*)bones) + (stride * startVtxIdx)) = b0;
			*(PxU32*)(((PxU8*)bones) + (stride * (startVtxIdx+1))) = b1;
			*(PxU32*)(((PxU8*)bones) + (stride * (startVtxIdx+2))) = b2;
		}

		if (colors)
		{
			const PxU32 c0 = *(PxU32*)(((PxU8*)colors) + (stride * outfaceIndices[ i]));
			const PxU32 c1 = *(PxU32*)(((PxU8*)colors) + (stride * outfaceIndices[ i+1]));
			const PxU32 c2 = *(PxU32*)(((PxU8*)colors) + (stride * outfaceIndices[ i+2]));

			*(PxU32*)(((PxU8*)colors) + (stride * startVtxIdx)) = c0;
			*(PxU32*)(((PxU8*)colors) + (stride * (startVtxIdx+1))) = c1;
			*(PxU32*)(((PxU8*)colors) + (stride * (startVtxIdx+2))) = c2;
		}

		if (normals)
		{
			PxVec3 v01 = p1 - p0;
			PxVec3 v02 = p2 - p0;
			v01.normalize();
			v02.normalize();
			PxVec3 n = v01.cross(v02);
			n.normalize();
			n = -n;

			*(PxVec3*)(((PxU8*)normals) + (stride * startVtxIdx)) = n;
			*(PxVec3*)(((PxU8*)normals) + (stride * (startVtxIdx+1))) = n;
			*(PxVec3*)(((PxU8*)normals) + (stride * (startVtxIdx+2))) = n;
		}		

		indices[ startIndexIdx] = startVtxIdx;
		indices[ startIndexIdx+1] = startVtxIdx+1;
		indices[ startIndexIdx+2] = startVtxIdx+2;

		startIndexIdx += 3;
		startVtxIdx += 3;
		i += 3;
	}
}

PxVec3 RandomOrthogonalVector(PxVec3 normal)
{
    PxVec3 random = CreateRandomVector(10);

    return random - (normal * random.dot(normal));
}

PX_FORCE_INLINE void collideWithSphere(PxsParticleCollData& collData, const PxSphereGeometry& sphereShapeData,
									   PxReal proxRadius)
{
	PxVec3& oldPos = collData.localOldPos;
	PxVec3& newPos = collData.localNewPos;

	PxReal radius = sphereShapeData.radius;

	PxReal oldPosDist2 = oldPos.magnitudeSquared();
	PxReal radius2 = radius * radius;

	bool oldInSphere = (oldPosDist2 < radius2);

	if(oldInSphere)
	{
		// old position inside the skeleton
		// add ccd with time 0.0

		collData.localSurfaceNormal = oldPos;
		if (oldPosDist2 > 0.0f)
			collData.localSurfaceNormal *= PxRecipSqrt(oldPosDist2);
		else
			collData.localSurfaceNormal = PxVec3(0,1.0f,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.ccTime = 0.0;
		collData.localFlags |= PXS_FLUID_COLL_FLAG_L_CC;
	}
	else
	{
		// old position is outside of the skeleton
		
		PxVec3 motion = newPos - oldPos;

		// Discriminant
		PxReal b = motion.dot(oldPos) * 2.0f;
		PxReal a2 = 2.0f * motion.magnitudeSquared();
		PxReal disc = (b*b) - (2.0f * a2 * (oldPosDist2 - radius2));

		bool intersection = disc > 0.0f;

		if ((!intersection) || (a2 == 0.0f))
		{
			// the ray does not intersect the sphere
			collideWithSphereNonContinuous(collData, newPos, radius, proxRadius);
		}
		else
		{
			// the ray intersects the sphere
			PxReal t = -(b + PxSqrt(disc)) / a2;	// Compute intersection point
			
			if (t < 0.0f || t > 1.0f)
			{
				// intersection point lies outside motion vector
				collideWithSphereNonContinuous(collData, newPos, radius, proxRadius);
			}
			else if(t < collData.ccTime)
			{
				// intersection point lies on sphere, add lcc
				//collData.localSurfacePos = oldPos + (motion * t);
				//collData.localSurfaceNormal = collData.localSurfacePos;
				//collData.localSurfaceNormal *= (1.0f / radius);
				//collData.localSurfacePos += (collData.localSurfaceNormal * collData.restOffset);
				PxVec3 relativeImpact = motion*t;
				collData.localSurfaceNormal = oldPos + relativeImpact;
				collData.localSurfaceNormal *= (1.0f / radius);		
				computeContinuousTargetPosition(collData.localSurfacePos, collData.localOldPos, relativeImpact, collData.localSurfaceNormal, collData.restOffset);
				
				collData.ccTime = t;
				collData.localFlags |= PXS_FLUID_COLL_FLAG_L_CC;
			}
		}
	}
}

bool Gu::intersectOBBOBB(const PxVec3& e0, const PxVec3& c0, const PxMat33& r0, const PxVec3& e1, const PxVec3& c1, const PxMat33& r1, bool full_test)
{
	// Translation, in parent frame
	const PxVec3 v = c1 - c0;
	// Translation, in A's frame
	const PxVec3 T(v.dot(r0[0]), v.dot(r0[1]), v.dot(r0[2]));

	// B's basis with respect to A's local frame
	PxReal R[3][3];
	PxReal FR[3][3];
	PxReal ra, rb, t;

	// Calculate rotation matrix
	for(PxU32 i=0;i<3;i++)
	{
		for(PxU32 k=0;k<3;k++)
		{
			R[i][k] = r0[i].dot(r1[k]);
			FR[i][k] = 1e-6f + PxAbs(R[i][k]);	// Precompute fabs matrix
		}
	}

	// A's basis vectors
	for(PxU32 i=0;i<3;i++)
	{
		ra = e0[i];

		rb = e1[0]*FR[i][0] + e1[1]*FR[i][1] + e1[2]*FR[i][2];

		t = PxAbs(T[i]);

		if(t > ra + rb)		return false;
	}

	// B's basis vectors
	for(PxU32 k=0;k<3;k++)
	{
		ra = e0[0]*FR[0][k] + e0[1]*FR[1][k] + e0[2]*FR[2][k];

		rb = e1[k];

		t = PxAbs(T[0]*R[0][k] + T[1]*R[1][k] + T[2]*R[2][k]);

		if( t > ra + rb )	return false;
	}

	if(full_test)
	{
		//9 cross products

		//L = A0 x B0
		ra	= e0[1]*FR[2][0] + e0[2]*FR[1][0];
		rb	= e1[1]*FR[0][2] + e1[2]*FR[0][1];
		t	= PxAbs(T[2]*R[1][0] - T[1]*R[2][0]);
		if(t > ra + rb)	return false;

		//L = A0 x B1
		ra	= e0[1]*FR[2][1] + e0[2]*FR[1][1];
		rb	= e1[0]*FR[0][2] + e1[2]*FR[0][0];
		t	= PxAbs(T[2]*R[1][1] - T[1]*R[2][1]);
		if(t > ra + rb)	return false;

		//L = A0 x B2
		ra	= e0[1]*FR[2][2] + e0[2]*FR[1][2];
		rb	= e1[0]*FR[0][1] + e1[1]*FR[0][0];
		t	= PxAbs(T[2]*R[1][2] - T[1]*R[2][2]);
		if(t > ra + rb)	return false;

		//L = A1 x B0
		ra	= e0[0]*FR[2][0] + e0[2]*FR[0][0];
		rb	= e1[1]*FR[1][2] + e1[2]*FR[1][1];
		t	= PxAbs(T[0]*R[2][0] - T[2]*R[0][0]);
		if(t > ra + rb)	return false;

		//L = A1 x B1
		ra	= e0[0]*FR[2][1] + e0[2]*FR[0][1];
		rb	= e1[0]*FR[1][2] + e1[2]*FR[1][0];
		t	= PxAbs(T[0]*R[2][1] - T[2]*R[0][1]);
		if(t > ra + rb)	return false;

		//L = A1 x B2
		ra	= e0[0]*FR[2][2] + e0[2]*FR[0][2];
		rb	= e1[0]*FR[1][1] + e1[1]*FR[1][0];
		t	= PxAbs(T[0]*R[2][2] - T[2]*R[0][2]);
		if(t > ra + rb)	return false;

		//L = A2 x B0
		ra	= e0[0]*FR[1][0] + e0[1]*FR[0][0];
		rb	= e1[1]*FR[2][2] + e1[2]*FR[2][1];
		t	= PxAbs(T[1]*R[0][0] - T[0]*R[1][0]);
		if(t > ra + rb)	return false;

		//L = A2 x B1
		ra	= e0[0]*FR[1][1] + e0[1]*FR[0][1];
		rb	= e1[0] *FR[2][2] + e1[2]*FR[2][0];
		t	= PxAbs(T[1]*R[0][1] - T[0]*R[1][1]);
		if(t > ra + rb)	return false;

		//L = A2 x B2
		ra	= e0[0]*FR[1][2] + e0[1]*FR[0][2];
//.........这里部分代码省略.........

//.........这里部分代码省略.........
			{
				const Gu::ContactPoint& contact = contactBase[j];

				SolverContactPointExt* PX_RESTRICT solverContact = reinterpret_cast<SolverContactPointExt*>(p);
				p += pointStride;

				setupExtSolverContact(b0, b1, d0, d1, angD0, angD1, bodyFrame0, bodyFrame1, normal, invDtV, invDtp8, restDistance, maxPenBias, restitution,
					bounceThreshold, contact, *solverContact, ccdMaxSeparation);
			}			
			ptr = p;
		}
	}

	//construct all the frictions

	PxU8* PX_RESTRICT ptr2 = workspace;

	const PxF32 orthoThreshold = 0.70710678f;
	const PxF32 eps = 0.00001f;
	bool hasFriction = false;

	for(PxU32 i=0;i< frictionPatchCount;i++)
	{
		const PxU32 contactCount = c.frictionPatchContactCounts[i];
		if(contactCount == 0)
			continue;

		SolverContactCoulombHeader* header = reinterpret_cast<SolverContactCoulombHeader*>(ptr2); 
		header->frictionOffset = PxU16(ptr - ptr2);
		ptr2 += sizeof(SolverContactCoulombHeader) + header->numNormalConstr * pointStride;

		const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;

		PxVec3 normal = contactBase0->normal;

		const PxReal staticFriction = contactBase0->staticFriction;
		const bool disableStrongFriction = !!(contactBase0->materialFlags & PxMaterialFlag::eDISABLE_FRICTION);
		const bool haveFriction = (disableStrongFriction == 0);
	
		SolverFrictionHeader* frictionHeader = reinterpret_cast<SolverFrictionHeader*>(ptr);
		frictionHeader->numNormalConstr = Ps::to8(c.frictionPatchContactCounts[i]);
		frictionHeader->numFrictionConstr = Ps::to8(haveFriction ? c.frictionPatchContactCounts[i] * frictionCountPerPoint : 0);
		frictionHeader->flags = flags;
		ptr += sizeof(SolverFrictionHeader);
		PxF32* forceBuffer = reinterpret_cast<PxF32*>(ptr);
		ptr += frictionHeader->getAppliedForcePaddingSize(c.frictionPatchContactCounts[i]);
		PxMemZero(forceBuffer, sizeof(PxF32) * c.frictionPatchContactCounts[i]);
		Ps::prefetchLine(ptr, 128);
		Ps::prefetchLine(ptr, 256);
		Ps::prefetchLine(ptr, 384);


		const PxVec3 t0Fallback1(0.f, -normal.z, normal.y);
		const PxVec3 t0Fallback2(-normal.y, normal.x, 0.f) ;
		const PxVec3 tFallback1 = orthoThreshold > PxAbs(normal.x) ? t0Fallback1 : t0Fallback2;
		const PxVec3 vrel = b0.getLinVel() - b1.getLinVel();
		const PxVec3 t0_ = vrel - normal * (normal.dot(vrel));
		const PxReal sqDist = t0_.dot(t0_);
		const PxVec3 tDir0 = (sqDist > eps ? t0_: tFallback1).getNormalized();
		const PxVec3 tDir1 = tDir0.cross(normal);
		PxVec3 tFallback[2] = {tDir0, tDir1};

		PxU32 ind = 0;

		if(haveFriction)
		{