C++ Point::Magnitude方法代码示例

udword PhysX::BatchCapsuleSweeps(PintSQThreadContext context, udword nb, PintRaycastHit* dest, const PintCapsuleSweepData* sweeps)
{
	ASSERT(mScene);

	const PxQueryFilterData PF = GetSQFilterData();
	const PxSceneQueryFlags sweepQueryFlags = GetSweepQueryFlags(mParams);

	udword NbHits = 0;
	while(nb--)
	{
		const Point Center = (sweeps->mCapsule.mP0 + sweeps->mCapsule.mP1)*0.5f;
		Point CapsuleAxis = sweeps->mCapsule.mP1 - sweeps->mCapsule.mP0;
		const float M = CapsuleAxis.Magnitude();
		CapsuleAxis /= M;
		const PxQuat q = PxShortestRotation(PxVec3(1.0f, 0.0f, 0.0f), ToPxVec3(CapsuleAxis));

		const PxTransform Pose(ToPxVec3(Center), q);

		PxSweepHit Hit;
		if(mScene->sweepSingle(PxCapsuleGeometry(sweeps->mCapsule.mRadius, M*0.5f), Pose, ToPxVec3(sweeps->mDir), sweeps->mMaxDist, sweepQueryFlags, Hit, PF))
		{
			NbHits++;
			FillResultStruct(*dest, Hit);
		}
		else
		{
			dest->mObject = null;
		}

		sweeps++;
		dest++;
	}
	return NbHits;
}

Matrix& Matrix::ComputeAxisMatrix(Point& axis, float angle)
{
	MakeIdentity();

	float length = axis.Magnitude();
	// Normalize the z basis vector3
	axis /= length;

	// Get the dot product, and calculate the projection of the z basis
	// vector3 onto the up vector3. The projection is the y basis vector3.
	float dotProduct = Point(0, 1, 0) | axis;
	Point Up = Point(0, 1, 0) - dotProduct * axis;

	// This is to prevent bogus view matrix (up view vector3 equals to axis)
	if (Up.Magnitude() < 1e-6f)	{
		Up = Point(0, 0, 1);
	}
	else	{
	// Normalize the y basis vector3
		Up /= length;
		Up.Normalize();
	}

	// The x basis vector3 is found simply with the cross product of the y
	// and z basis vectors
	Point Right = Up ^ axis;

	SetCol( 0, Right );
	SetCol( 1, Up );
	SetCol( 2, axis );
	Transpose();

	return *this;
}

	//**************************************
	// Angle between two vectors (in radians)
	// we use this formula
	// uv = |u||v| cos(u,v)
	// u  ^ v  = w
	// |w| = |u||v| |sin(u,v)|
	//**************************************
	float Angle(const Point& u, const Point& v)
	{
		float NormU = u.Magnitude();	// |u|
		float NormV = v.Magnitude();	// |v|
		float Product = NormU*NormV;	// |u||v|
		if(Product==0.0f)	return 0.0f;
		float OneOverProduct = 1.0f / Product;

		// Cosinus
		float Cosinus = (u|v) * OneOverProduct;

		// Sinus
		Point w = u^v;
		float NormW = w.Magnitude();

		float AbsSinus = NormW * OneOverProduct;

		// Remove degeneracy
		if(AbsSinus > 1.0f) AbsSinus = 1.0f;
		if(AbsSinus < -1.0f) AbsSinus = -1.0f;

		if(Cosinus>=0.0f)	return asinf(AbsSinus);
		else				return (PI-asinf(AbsSinus));
	}

void IceMaths::NormalizePRSMatrix(Matrix4x4& dest, Point& scale, const Matrix4x4& src)
{
	Point row;
	dest = src;
	for( int i=0;i<3;i++)
	{
		src.GetRow(i,row);

		// computes scales

		scale[i] = row.Magnitude();

		row /= scale[i];

		dest.SetRow(i,row);
	}
}

// Vector normalization can handle a Near-Zero Magnitude
TEST_F(PointTest, NormNearZeroMag) {
  Point p = NearZeroMag_; 
  p = p.Normalize();
  EXPECT_FALSE(p.Equals(ZeroMag_)) << "Normalize near 0 vector == 0 vector";
  ASSERT_EQ(p.Magnitude(), 1)  << "Normalize near 0 vector: Mag != 1";
}