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

void RenderPhysX3Debug::addArrow(const PxVec3& posA, const PxVec3& posB, const RendererColor& color)
{
	const PxVec3 t0 = (posB - posA).getNormalized();
	const PxVec3 a = PxAbs(t0.x)<0.707f ? PxVec3(1,0,0): PxVec3(0,1,0);
	const PxVec3 t1 = t0.cross(a).getNormalized();
	const PxVec3 t2 = t0.cross(t1).getNormalized();

	addLine(posA, posB, color);
	addLine(posB, posB - t0*0.15 + t1 * 0.15, color);
	addLine(posB, posB - t0*0.15 - t1 * 0.15, color);
	addLine(posB, posB - t0*0.15 + t2 * 0.15, color);
	addLine(posB, posB - t0*0.15 - t2 * 0.15, color);
}

bool Actor::patternFracture(const PxVec3& hitLocation, const PxVec3& dirIn, float scale, float vel, float radiusIn)
{
	int compoundNr = -1;
	int convexNr = -1;
	PxVec3 normal;
	float dist;
	bool ret = false;
	PxVec3 dir = dirIn;

	mScene->getScene()->lockWrite();

	bool hit = false;
	if (dir.magnitudeSquared() < 0.5f)
	{
		dir = PxVec3(1.f,0.f,0.f);
		hit = base::Actor::rayCast(hitLocation-dir,dir,dist,compoundNr,convexNr,normal);
		if(!hit)
		{
			dir = PxVec3(0.f,1.f,0.f);
			hit = base::Actor::rayCast(hitLocation-dir,dir,dist,compoundNr,convexNr,normal);
			if(!hit)
			{
				dir = PxVec3(0.f,0.f,1.f);
				hit = base::Actor::rayCast(hitLocation-dir,dir,dist,compoundNr,convexNr,normal);
			}
		}
	}
	else
	{
		hit = base::Actor::rayCast(hitLocation-dir,dir,dist,compoundNr,convexNr,normal);
	}
	if (hit)
	{
		float radius = mMinRadius + mRadiusMultiplier*radiusIn;
		float impulseMagn = scale*vel*mImpulseScale;

		if (mSheetFracture)
		{
			normal = ((Compound*)mCompounds[(uint32_t)compoundNr])->mNormal;
		}
		PxVec3 a(0.f,0.f,1.f);
		normal.normalize();
		a -= a.dot(normal)*normal;
		if( a.magnitudeSquared() < 0.1f )
		{
			a = PxVec3(0.f,1.f,0.f);
			a -= a.dot(normal)*normal;
		}
		a.normalize();
		PxVec3 b(normal.cross(a));
		PxMat33 trans(a,b,normal);
		ret = base::Actor::patternFracture(hitLocation,dir,compoundNr,trans,radius,impulseMagn,impulseMagn);
	}

	mScene->getScene()->unlockWrite();

	mRenderResourcesDirty = true;

	return ret;
}

// -------------------------------------------------------------------------------------
void PolygonTriangulator::importPoints(const PxVec3 *points, int numPoints, const int *indices, PxVec3 *planeNormal, bool &isConvex)
{
	// find projection 3d -> 2d;
	PxVec3 n;

	isConvex = true;

	if (planeNormal) 
		n = *planeNormal;
	else {
		PX_ASSERT(numPoints >= 3);
		n = PxVec3(0.0f, 0.0f, 0.0f);

		for (int i = 1; i < numPoints-1; i++) {
			int i0 = 0;
			int i1 = i;
			int i2 = i+1;
			if (indices) {
				i0 = indices[i0];
				i1 = indices[i1];
				i2 = indices[i2];
			}
			const PxVec3 &p0 = points[i0];
			const PxVec3 &p1 = points[i1];
			const PxVec3 &p2 = points[i2];
			PxVec3 ni = (p1-p0).cross(p2-p0);
			if (i > 1 && ni.dot(n) < 0.0f)
				isConvex = false;
			n += ni;
		}
	}

	n.normalize();
	PxVec3 t0,t1;

	if (fabs(n.x) < fabs(n.y) && fabs(n.x) < fabs(n.z))
		t0 = PxVec3(1.0f, 0.0f, 0.0f);
	else if (fabs(n.y) < fabs(n.z))
		t0 = PxVec3(0.0f, 1.0f, 0.0f);
	else
		t0 = PxVec3(0.0f, 0.0f, 1.0f);
	t1 = n.cross(t0);
	t1.normalize();
	t0 = t1.cross(n);
	
	mPoints.resize((uint32_t)numPoints);
	if (indices == NULL) {
		for (uint32_t i = 0; i < (uint32_t)numPoints; i++) 
			mPoints[i] = PxVec3(points[i].dot(t0), points[i].dot(t1), 0.0f);
	}
	else {
		for (uint32_t i = 0; i < (uint32_t)numPoints; i++) {
			const PxVec3 &p = points[(uint32_t)indices[i]];
			mPoints[i] = PxVec3(p.dot(t0), p.dot(t1), 0.0f);
		}
	}
}

static PxQuat directionToQuaternion(const PxVec3& direction) 
{
    PxVec3 vUp(0.0f, 1.0f, 0.0f);
    PxVec3 vRight = vUp.cross(direction);
    vUp = direction.cross(vRight);

	PxQuat qrot(PxMat33(vRight, vUp, direction));
	qrot.normalize();

    return qrot;
}

void SampleNorthPoleCameraController::update(Camera& camera, PxReal dtime)
{
	// Update CCT
	if(!mBase.isPaused())
	{
		PxVec3 targetKeyDisplacement(0);
		PxVec3 targetPadDisplacement(0);

		PxVec3 forward = camera.getViewDir();
		forward.y = 0;
		forward.normalize();
		PxVec3 up = PxVec3(0,1,0);
		PxVec3 right = forward.cross(up);

		if(mFwd)	targetKeyDisplacement += forward;
		if(mBwd)	targetKeyDisplacement -= forward;

		if(mRight)	targetKeyDisplacement += right;
		if(mLeft)	targetKeyDisplacement -= right;

		targetKeyDisplacement *= mKeyShiftDown ? mRunningSpeed : mWalkingSpeed;
		targetKeyDisplacement += PxVec3(0,-9.81,0);
		targetKeyDisplacement *= dtime;

		targetPadDisplacement += forward * mGamepadForwardInc * mRunningSpeed;
		targetPadDisplacement += right * mGamepadLateralInc * mRunningSpeed;
		targetPadDisplacement += PxVec3(0,-9.81,0);
		targetPadDisplacement *= dtime;

		PxU32 flags = mCCT.move(targetKeyDisplacement + targetPadDisplacement, 0.001f, dtime, PxControllerFilters(0));
		PX_UNUSED(flags);
	}
	// Update camera
	{
		mTargetYaw		+= mGamepadYawInc * dtime;
		mTargetPitch	+= mGamepadPitchInc * dtime;
		
		// Clamp pitch
		if(mTargetPitch<mPitchMin)	mTargetPitch = mPitchMin;
		if(mTargetPitch>mPitchMax)	mTargetPitch = mPitchMax;
		
		camera.setRot(PxVec3(-mTargetPitch,-mTargetYaw,0));

		PxExtendedVec3 camTarget = computeCameraTarget();
		const float filteredHeight = feedbackFilter((float)camTarget.y, mFilterMemory, dtime*6.0f);
		camTarget.y = filteredHeight;

		const PxF32 distanceToTarget = 0.0f;
		const PxVec3 target = toVec3(camTarget) - camera.getViewDir()*distanceToTarget;
		camera.setPos(target);
	}
}

bool Gu::intersectEdgeEdge(const PxVec3& p1, const PxVec3& p2, const PxVec3& dir, const PxVec3& p3, const PxVec3& p4, PxReal& dist, PxVec3& ip)
{
	const PxVec3 v1 = p2 - p1;

	// Build plane P based on edge (p1, p2) and direction (dir)
	Gu::Plane plane;
	plane.normal = v1.cross(dir);
	plane.d = -(plane.normal.dot(p1));

	// if colliding edge (p3,p4) does not cross plane return no collision
	// same as if p3 and p4 on same side of plane return 0
	//
	// Derivation:
	// d3 = d(p3, P) = (p3 | plane.n) - plane.d;		Reversed sign compared to Plane::Distance() because plane.d is negated.
	// d4 = d(p4, P) = (p4 | plane.n) - plane.d;		Reversed sign compared to Plane::Distance() because plane.d is negated.
	// if d3 and d4 have the same sign, they're on the same side of the plane => no collision
	// We test both sides at the same time by only testing Sign(d3 * d4).
	// ### put that in the Plane class
	// ### also check that code in the triangle class that might be similar
	const PxReal d3 = plane.distance(p3);
	PxReal temp = d3 * plane.distance(p4);
	if(temp>0.0f)	return false;

	// if colliding edge (p3,p4) and plane are parallel return no collision
	PxVec3 v2 = p4 - p3;

	temp = plane.normal.dot(v2);
	if(temp==0.0f)	return false;	// ### epsilon would be better

	// compute intersection point of plane and colliding edge (p3,p4)
	ip = p3-v2*(d3/temp);

	// find largest 2D plane projection
	PxU32 i,j;
	Ps::closestAxis(plane.normal, i, j);

	// compute distance of intersection from line (ip, -dir) to line (p1,p2)
	dist =	(v1[i]*(ip[j]-p1[j])-v1[j]*(ip[i]-p1[i]))/(v1[i]*dir[j]-v1[j]*dir[i]);
	if(dist<0.0f)	return false;

	// compute intersection point on edge (p1,p2) line
	ip -= dist*dir;

	// check if intersection point (ip) is between edge (p1,p2) vertices
	temp = (p1.x-ip.x)*(p2.x-ip.x)+(p1.y-ip.y)*(p2.y-ip.y)+(p1.z-ip.z)*(p2.z-ip.z);
	if(temp<0.0f)	return true;	// collision found

	return false;	// no collision
}

PX_INLINE void computeFrictionTangents(const PxVec3& vrel,const PxVec3& unitNormal, PxVec3& t0, PxVec3& t1)
{
	PX_ASSERT(PxAbs(unitNormal.magnitude()-1)<1e-3f);

	t0 = vrel - unitNormal * unitNormal.dot(vrel);
	PxReal ll = t0.magnitudeSquared();

	if (ll > 0.1f)										//can set as low as 0.
	{
		t0 *= PxRecipSqrt(ll);
		t1 = unitNormal.cross(t0);
	}
	else
		Ps::normalToTangents(unitNormal, t0, t1);		//fallback
}

void CookingAbstract::PhysicalMesh::computeTriangleAreas()
{
	smallestTriangleArea = largestTriangleArea = 0.0f;

	if (indices == NULL || vertices == NULL)
	{
		return;
	}

	smallestTriangleArea = PX_MAX_F32;

	for (PxU32 i = 0; i < numIndices; i += 3)
	{
		const PxVec3 edge1 = vertices[indices[i + 1]] - vertices[indices[i]];
		const PxVec3 edge2 = vertices[indices[i + 2]] - vertices[indices[i]];
		const PxF32 triangleArea = edge1.cross(edge2).magnitude();

		largestTriangleArea = PxMax(largestTriangleArea, triangleArea);
		smallestTriangleArea = PxMin(smallestTriangleArea, triangleArea);
	}
}

bool Character::faceToward(const PxVec3& targetDir, PxReal angleLimitPerFrame)
{
	PxVec3 oldDir = mCharacterPose.q.rotate(PxVec3(0,0,1));
	PxVec3 up(0,1,0);
	PxVec3 newDir = PxVec3(targetDir.x, 0, targetDir.z).getNormalized();
	PxVec3 right = -1.0f * oldDir.cross(up);

	PxReal cos = newDir.dot(oldDir);
	PxReal sin = newDir.dot(right);
	PxReal angle = atan2(sin, cos);

	PxReal limit = angleLimitPerFrame * (PxPi / 180.0f);
	if (angle > limit) angle = limit;
	else if (angle < -limit) angle = -limit;

	PxQuat qdel(angle, up);

	mCharacterPose.q = qdel * mCharacterPose.q;

	return true;
}

bool Gu::intersectEdgeEdgeNEW(const PxVec3& p1, const PxVec3& p2, const PxVec3& dir, const PxVec3& p3, const PxVec3& p4, PxReal& dist, PxVec3& ip)
{
	// Build plane P based on edge (p1, p2) and direction (dir)
	const PxVec3 v12 = p2-p1;

	Gu::Plane plane;
	plane.normal = v12.cross(dir);
	plane.d = -(plane.normal.dot(p1));

	PxReal d3 = plane.distance(p3);
	PxReal d4 = plane.distance(p4);

	// line doesn't intersect plane if both have same sign or both are 0.
	if(d3*d4>0 || d3==d4) 
		return false;

	// vector from p1 to intersection point of plane and edge (p3,p4)
	PxVec3 v1i = (d3*p4 - d4*p3)/(d3-d4) - p1;

	// find largest 2D plane projection
	PxU32 i,j;
	Ps::closestAxis(plane.normal, i, j);

	// compute distance of v1i to intersection of line (v1i, v1i-dir) and line (0,v12)
	PxReal d = (v12[i]*v1i[j]-v12[j]*v1i[i])/(v12[i]*dir[j]-v12[j]*dir[i]);
	if(d<0.0f)
		return false;

	// vector from p1 to intersection point of two lines above
	v1i -= d*dir;

	// we are allowed to write invalid output
	dist = d;
	ip = p1 + v1i;

	// return if intersection point is on sweep side and between p1 and p2
	return v1i.dot(v1i-v12)<0.0f;
}

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

		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)
		{
			hasFriction = true;
			frictionHeader->setStaticFriction(staticFriction);
			frictionHeader->invMass0D0 = d0;
			frictionHeader->invMass1D1 = d1;
			frictionHeader->angDom0 = angD0;
			frictionHeader->angDom1 = angD1;
			frictionHeader->type			= frictionHeaderType;
			
			PxU32 totalPatchContactCount = 0;
		
			for(PxU32 patch=c.correlationListHeads[i]; 
				patch!=CorrelationBuffer::LIST_END; 
				patch = c.contactPatches[patch].next)
			{
				const PxU32 count = c.contactPatches[patch].count;
				const PxU32 start = c.contactPatches[patch].start;
				const Gu::ContactPoint* contactBase = buffer.contacts + start;
					
				PxU8* p = ptr;

				for(PxU32 j =0; j < count; j++)
				{
					const Gu::ContactPoint& contact = contactBase[j];
					const PxVec3 ra = contact.point - bodyFrame0.p;
					const PxVec3 rb = contact.point - bodyFrame1.p;
						

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

				for(PxU32 pi = 0; pi < npcp; pi++)
				{
					PX_ASSERT(closestFeatures[pi] != 0xffffffff);
					const PxVec3 d = sphereInHfShape - closestPoints[pi];

					if (hf.isDeltaHeightOppositeExtent(d.y)) // See if we are 'above' the heightfield
					{
						const PxReal dMagSq = d.magnitudeSquared();

						if (dMagSq > inflatedRadiusSquared) 
							// Too far above
							continue;

						PxReal dMag = -1.0f; // dMag is sqrt(sMadSq) and comes up as a byproduct of other calculations in computePointNormal
						PxVec3 n; // n is in world space, rotated by transform1
						PxU32 featureType = HFU::getFeatureType(closestFeatures[pi]);
						if (featureType == HFU::eEDGE)
						{
							PxU32 edgeIndex = HFU::getFeatureIndex(closestFeatures[pi]);
							PxU32 adjFaceIndices[2];
							const PxU32 adjFaceCount = hf.getEdgeTriangleIndices(edgeIndex, adjFaceIndices);
							PxVec3 origin;
							PxVec3 direction;
							const PxU32 vertexIndex = edgeIndex / 3;
							const PxU32 row			= vertexIndex / nbColumns;
							const PxU32 col			= vertexIndex % nbColumns;
							hfUtil.getEdge(edgeIndex, vertexIndex, row, col, origin, direction);
							n = hfUtil.computePointNormal(
									hfGeom.heightFieldFlags, d, transform1, dMagSq,
									closestPoints[pi].x, closestPoints[pi].z, epsSqr, dMag);
							PxVec3 localN = transform1.rotateInv(n);
							// clamp the edge's normal to its Voronoi region
							for (PxU32 j = 0; j < adjFaceCount; j++)
							{
								const PxVec3 adjNormal = hfUtil.hf2shapen(hf.getTriangleNormalInternal(adjFaceIndices[j])).getNormalized();
								PxU32 triCell = adjFaceIndices[j] >> 1;
								PxU32 triRow = triCell/hf.getNbColumnsFast();
								PxU32 triCol = triCell%hf.getNbColumnsFast();
								PxVec3 tv0, tv1, tv2, tvc;
								hf.getTriangleVertices(adjFaceIndices[j], triRow, triCol, tv0, tv1, tv2);
								tvc = hfUtil.hf2shapep((tv0+tv1+tv2)/3.0f); // compute adjacent triangle center
								PxVec3 perp = adjNormal.cross(direction).getNormalized(); // adj face normal cross edge dir
								if (perp.dot(tvc-origin) < 0.0f) // make sure perp is pointing toward the center of the triangle
									perp = -perp;
								// perp is now a vector sticking out of the edge of the triangle (also the test edge) pointing toward the center
								// perpendicular to the normal (in triangle plane)
								if (perp.dot(localN) > 0.0f) // if the normal is in perp halfspace, clamp it to Voronoi region
								{
									n = transform1.rotate(adjNormal);
									break;
								}
							}
						} else if(featureType == HFU::eVERTEX)
						{
							// AP: these contacts are rare so hopefully it's ok
							const PxU32 bufferCount = contactBuffer.count;
							const PxU32 vertIndex = HFU::getFeatureIndex(closestFeatures[pi]);
							EdgeData adjEdges[8];
							const PxU32 row = vertIndex / nbColumns;
							const PxU32 col = vertIndex % nbColumns;
							const PxU32 numAdjEdges = ::getVertexEdgeIndices(hf, vertIndex, row, col, adjEdges);
							for (PxU32 iPrevEdgeContact = numFaceContacts; iPrevEdgeContact < bufferCount; iPrevEdgeContact++)
							{
								if (contactBuffer.contacts[iPrevEdgeContact].forInternalUse != HFU::eEDGE)
									continue; // skip non-edge contacts (can be other vertex contacts)

								for (PxU32 iAdjEdge = 0; iAdjEdge < numAdjEdges; iAdjEdge++)
									// does adjacent edge index for this vertex match a previously encountered edge index?
									if (adjEdges[iAdjEdge].edgeIndex == contactBuffer.contacts[iPrevEdgeContact].internalFaceIndex1)
									{
										// if so, clamp the normal for this vertex to that edge's normal
										n = contactBuffer.contacts[iPrevEdgeContact].normal;
										dMag = PxSqrt(dMagSq);
										break;
									}
							}
						}

						if (dMag == -1.0f)
							n = hfUtil.computePointNormal(hfGeom.heightFieldFlags, d, transform1,
								dMagSq, closestPoints[pi].x, closestPoints[pi].z, epsSqr, dMag);

						PxVec3 p = transform0.p - n * radius;
							#if DEBUG_RENDER_HFCONTACTS
							printf("n=%.5f %.5f %.5f;  ", n.x, n.y, n.z);
							if (n.y < 0.8f)
								int a = 1;
							PxScene *s; PxGetPhysics().getScenes(&s, 1, 0);
							Cm::RenderOutput((Cm::RenderBuffer&)s->getRenderBuffer()) << Cm::RenderOutput::LINES << PxDebugColor::eARGB_BLUE // red
								<< p << (p + n * 10.0f);
							#endif

						// temporarily use the internalFaceIndex0 slot in the contact buffer for featureType
	 					contactBuffer.contact(
							p, n, dMag - radius, PxU16(featureType), HFU::getFeatureIndex(closestFeatures[pi]));	
					}
				}
			}
		}

//.........这里部分代码省略.........
		if(haveFriction)
		{
			//const Vec3V normal = Vec3V_From_PxVec3(buffer.contacts[c.contactPatches[c.correlationListHeads[i]].start].normal);
			PxVec3 normalS = buffer[c.contactPatches[c.correlationListHeads[i]].start].normal;

			PxVec3 t0, t1;
			computeFrictionTangents(b0.getLinVel() - b1.getLinVel(), normalS, t0, t1);

			Vec3V vT0 = V3LoadU(t0);
			Vec3V vT1 = V3LoadU(t1);
			
			//We want to set the writeBack ptr to point to the broken flag of the friction patch.
			//On spu we have a slight problem here because the friction patch array is 
			//in local store rather than in main memory. The good news is that the address of the friction 
			//patch array in main memory is stored in the work unit. These two addresses will be equal 
			//except on spu where one is local store memory and the other is the effective address in main memory.
			//Using the value stored in the work unit guarantees that the main memory address is used on all platforms.
			PxU8* PX_RESTRICT writeback = frictionDataPtr + frictionPatchWritebackAddrIndex*sizeof(FrictionPatch);

			header->frictionBrokenWritebackByte = writeback;			

			for(PxU32 j = 0; j < frictionPatch.anchorCount; j++)
			{
				SolverContactFrictionExt* PX_RESTRICT f0 = reinterpret_cast<SolverContactFrictionExt*>(ptr);
				ptr += frictionStride;
				SolverContactFrictionExt* PX_RESTRICT f1 = reinterpret_cast<SolverContactFrictionExt*>(ptr);
				ptr += frictionStride;

				PxVec3 ra = bodyFrame0.q.rotate(frictionPatch.body0Anchors[j]);
				PxVec3 rb = bodyFrame1.q.rotate(frictionPatch.body1Anchors[j]);
				PxVec3 error = (ra + bodyFrame0.p) - (rb + bodyFrame1.p);

				{
					const PxVec3 raXn = ra.cross(t0);
					const PxVec3 rbXn = rb.cross(t0);

					Cm::SpatialVector deltaV0, deltaV1;

					const Cm::SpatialVector resp0 = createImpulseResponseVector(t0, raXn, b0);
					const Cm::SpatialVector resp1 = createImpulseResponseVector(-t1, -rbXn, b1);
					FloatV resp = FLoad(getImpulseResponse(b0, resp0, deltaV0, d0, angD0,
															 b1, resp1, deltaV1, d1, angD1));

					const FloatV velMultiplier = FSel(FIsGrtr(resp, zero), FMul(p8, FRecip(resp)), zero);

					PxU32 index = c.contactPatches[c.correlationListHeads[i]].start;
					PxF32 targetVel = buffer[index].targetVel.dot(t0);

					if(b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
						targetVel -= b0.projectVelocity(t0, raXn);
					else if(b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
						targetVel += b1.projectVelocity(t0, rbXn);

					f0->normalXYZ_appliedForceW = V4SetW(vT0, zero);
					f0->raXnXYZ_velMultiplierW = V4SetW(V4LoadA(&resp0.angular.x), velMultiplier);
					f0->rbXnXYZ_biasW = V4SetW(V4Neg(V4LoadA(&resp1.angular.x)), FLoad(t0.dot(error) * invDtF32));
					f0->linDeltaVA = V3LoadA(deltaV0.linear);
					f0->angDeltaVA = V3LoadA(deltaV0.angular);
					f0->linDeltaVB = V3LoadA(deltaV1.linear);
					f0->angDeltaVB = V3LoadA(deltaV1.angular);
					f0->targetVel = targetVel;
				}

				{

					const PxVec3 raXn = ra.cross(t1);

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

}

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