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";
}
// Normalization Tests
// Vector normalization can handle a Zero Magnitude
TEST_F(PointTest, NormZeroMag) {
Point p = ZeroMag_;
p = p.Normalize();
ASSERT_TRUE(p.Equals(ZeroMag_)) << "Normalize 0 vector != 0 vector";
ASSERT_EQ(p.Magnitude(), 0) << "Normalize 0 vector: Mag != 0";
}