Skip to content

Commit

Permalink
using intrinsic inertia instrad of the inertia that uses parallel axi…
Browse files Browse the repository at this point in the history 
…s theorem.
  • Loading branch information
@JulioJerez
JulioJerez committed Oct 29, 2024
1 parent 9f48b1e commit 9174373
Show file tree
Hide file tree
Showing 7 changed files with 126 additions and 160 deletions.
19 changes: 19 additions & 0 deletions ...newtonUnreal/newton/Source/ThirdParty/newtonLibrary/Public/dCollision/ndBodyKinematic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -284,6 +284,25 @@ void ndBodyKinematic::SetMassMatrix(ndFloat32 mass, const ndShapeInstance& shape
SetMassMatrix(mass, inertia);
}

void ndBodyKinematic::SetIntrinsicMassMatrix(ndFloat32 mass, const ndShapeInstance& shapeInstance, bool fullInertia)
{
const ndMatrix inertia(shapeInstance.CalculateInertia());
const ndVector saveCom(inertia.m_posit);
//SetCentreOfMass(inertia.m_posit);

ndShapeInstance instance(shapeInstance);
ndMatrix matrix(instance.GetLocalMatrix());
matrix.m_posit = ndVector::m_wOne;
if (instance.GetShape()->GetAsShapeConvexHull())
{
matrix.m_posit = saveCom * ndVector::m_negOne;
matrix.m_posit.m_w = ndFloat32 (1.0f);
}
instance.SetLocalMatrix(matrix);
SetMassMatrix(mass, instance, fullInertia);
SetCentreOfMass(saveCom);
}

void ndBodyKinematic::SetMassMatrix(ndFloat32 mass, const ndMatrix& inertia)
{
mass = ndAbs(mass);
Expand Down
2 changes: 2 additions & 0 deletions ...s/newtonUnreal/newton/Source/ThirdParty/newtonLibrary/Public/dCollision/ndBodyKinematic.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -135,6 +135,8 @@ class ndBodyKinematic : public ndBody
D_COLLISION_API virtual ndMatrix CalculateInvInertiaMatrix() const;
D_COLLISION_API virtual void SetMassMatrix(ndFloat32 mass, const ndMatrix& inertia);
D_COLLISION_API void SetMassMatrix(ndFloat32 mass, const ndShapeInstance& shapeInstance, bool fullInertia = false);
D_COLLISION_API void SetIntrinsicMassMatrix(ndFloat32 mass, const ndShapeInstance& shapeInstance, bool fullInertia = false);

D_COLLISION_API virtual void InitSurrogateBody(ndBodyKinematic* const surrogate) const;

void UpdateInvInertiaMatrix();
Expand Down
5 changes: 3 additions & 2 deletions ...ndWrapers/newtonUnreal/newton/Source/ThirdParty/newtonLibrary/Public/dCollision/ndShape.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,15 +39,15 @@ class ndShapeCylinder;
class ndShapeCompound;
class ndRayCastNotify;
class ndShapeInstance;
class ndShapeConvexHull;
class ndShapeStatic_bvh;
class ndShapeStaticMesh;
class ndShapeHeightfield;
class ndShapeConvexPolygon;
class ndShapeDebugNotify;
class ndShapeConvexPolygon;
class ndShapeChamferCylinder;
class ndShapeStaticProceduralMesh;


enum ndShapeID
{
// do not change the order of these enum
Expand Down Expand Up @@ -233,6 +233,7 @@ class ndShape: public ndContainersFreeListAlloc<ndShape>
virtual ndShapeCompound* GetAsShapeCompound() { return nullptr; }
virtual ndShapeStatic_bvh* GetAsShapeStaticBVH() { return nullptr; }
virtual ndShapeStaticMesh* GetAsShapeStaticMesh() { return nullptr; }
virtual ndShapeConvexHull* GetAsShapeConvexHull() { return nullptr; }
virtual ndShapeHeightfield* GetAsShapeHeightfield() { return nullptr; }
virtual ndShapeConvexPolygon* GetAsShapeAsConvexPolygon() { return nullptr; }
virtual ndShapeChamferCylinder* GetAsShapeChamferCylinder() { return nullptr; }
Expand Down
13 changes: 10 additions & 3 deletions ...s/newtonUnreal/newton/Source/ThirdParty/newtonLibrary/Public/dCollision/ndShapeConvex.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -190,9 +190,16 @@ ndFloat32 ndShapeConvex::CalculateMassProperties(const ndMatrix& offset, ndVecto

ndMatrix ndShapeConvex::CalculateInertiaAndCenterOfMass(const ndMatrix& alignMatrix, const ndVector& localScale, const ndMatrix& matrix) const
{
if ((ndAbs(localScale.m_x - localScale.m_y) < ndFloat32(1.0e-5f)) &&
(ndAbs(localScale.m_x - localScale.m_z) < ndFloat32(1.0e-5f)) &&
(ndAbs(localScale.m_y - localScale.m_z) < ndFloat32(1.0e-5f)))
bool implicitTest = true;
implicitTest = implicitTest && (ndAbs(localScale.m_x - localScale.m_y) < ndFloat32(1.0e-5f));
implicitTest = implicitTest && (ndAbs(localScale.m_x - localScale.m_z) < ndFloat32(1.0e-5f));
implicitTest = implicitTest && (ndAbs(localScale.m_y - localScale.m_z) < ndFloat32(1.0e-5f));
implicitTest = ((ndShape*)this)->GetAsShapeConvexHull() ? false : true;

//if ((ndAbs(localScale.m_x - localScale.m_y) < ndFloat32(1.0e-5f)) &&
// (ndAbs(localScale.m_x - localScale.m_z) < ndFloat32(1.0e-5f)) &&
// (ndAbs(localScale.m_y - localScale.m_z) < ndFloat32(1.0e-5f)))
if (implicitTest)
{
ndAssert(alignMatrix.TestIdentity());

Expand Down
2 changes: 2 additions & 0 deletions ...newtonUnreal/newton/Source/ThirdParty/newtonLibrary/Public/dCollision/ndShapeConvexHull.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@ class ndShapeConvexHull : public ndShapeConvex
D_COLLISION_API ndShapeConvexHull(ndInt32 count, ndInt32 strideInBytes, ndFloat32 tolerance, const ndFloat32* const vertexArray, ndInt32 maxPointsOut = 0x7fffffff);
D_COLLISION_API virtual ~ndShapeConvexHull();

virtual ndShapeConvexHull* GetAsShapeConvexHull() { return this; }

protected:
D_COLLISION_API ndShapeInfo GetShapeInfo() const;
D_COLLISION_API ndUnsigned64 GetHash(ndUnsigned64 hash) const;
Expand Down
244 changes: 90 additions & 154 deletions ...pplications/toolsAndWrapers/newtonUnreal/newton/Source/newton/Private/NewtonRigidBody.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -225,9 +225,9 @@ void UNewtonRigidBody::ClearDebug()
void UNewtonRigidBody::ActivateDebug()
{
ShowDebug = true;
ShowCenterOfMass = true;
//ShowCenterOfMass = true;
m_propertyChanged = true;
Inertia.ShowPrincipalAxis = true;
//Inertia.ShowPrincipalAxis = true;

ndFixSizeArray<USceneComponent*, 1024> stack;
stack.PushBack(this);
Expand All @@ -248,145 +248,63 @@ void UNewtonRigidBody::ActivateDebug()
}
}

ndMatrix UNewtonRigidBody::CalculateInertiaMatrix() const
{
ndMatrix inertia(ndGetZeroMatrix());
ndArray<ndShapeInstance*> instances;
ndFixSizeArray<const USceneComponent*, 1024> stack;
stack.PushBack(this);

bool isDynamics = true;
const ndMatrix bodyMatrix(ToNewtonMatrix(GetComponentToWorld()));
while (stack.GetCount())
{
const USceneComponent* const component = stack.Pop();
const UNewtonCollision* const collisionComponent = Cast<UNewtonCollision>(component);
if (collisionComponent)
{
check(collisionComponent->m_shape);
isDynamics = isDynamics || (!collisionComponent->m_shape->GetAsShapeStaticMesh()) ? true : false;

ndShapeInstance* const instance = collisionComponent->CreateBodyInstanceShape(bodyMatrix);
instances.PushBack(instance);
}
const TArray<TObjectPtr<USceneComponent>>& childrenComp = component->GetAttachChildren();
for (ndInt32 i = childrenComp.Num() - 1; i >= 0; --i)
{
stack.PushBack(childrenComp[i].Get());
}
}
if (instances.GetCount() == 1)
{
if (isDynamics)
{
inertia = instances[0]->CalculateInertia();
}
delete instances[0];
}
else if (instances.GetCount())
{
ndShapeInstance compoundInstance(new ndShapeCompound());
ndShapeCompound* const compound = compoundInstance.GetShape()->GetAsShapeCompound();
compound->BeginAddRemove();
for (ndInt32 i = ndInt32(instances.GetCount()) - 1; i >= 0; --i)
{
ndShapeInstance* const subShape = instances[i];
if (isDynamics)
{
compound->AddCollision(subShape);
}
delete subShape;
}
compound->EndAddRemove();
if (isDynamics)
{
inertia = compoundInstance.CalculateInertia();
}
}

return inertia;
}

void UNewtonRigidBody::DrawGizmo(float timestep)
{
if (Inertia.ShowPrincipalAxis)
if (Inertia.ShowPrincipalAxis && (Mass > 0.0f))
{
ndMatrix inertiaMatrix(CalculateInertiaMatrix());
const ndVector saveCom(inertiaMatrix.m_posit);
inertiaMatrix.EigenVectors();

const ndVector centerOfMass(
ndFloat32(CenterOfMass.X * UNREAL_INV_UNIT_SYSTEM),
ndFloat32(CenterOfMass.Y * UNREAL_INV_UNIT_SYSTEM),
ndFloat32(CenterOfMass.Z * UNREAL_INV_UNIT_SYSTEM),
ndFloat32(0.0f));
inertiaMatrix.m_posit = saveCom + centerOfMass;

const FTransform tranform(GetComponentToWorld());
const ndMatrix matrix(ToNewtonMatrix(tranform));

FTransform offsetInertia;
offsetInertia.SetRotation(Inertia.PrincipalInertiaAxis.Quaternion());
const ndMatrix offsetMatrix(ToNewtonMatrix(offsetInertia));

const ndMatrix axisMatrix(offsetMatrix * inertiaMatrix * matrix);
const FTransform inertiaAxisTransform (ToUnRealTransform(axisMatrix));
const FVector axisLoc(inertiaAxisTransform.GetLocation());
const FRotator axisRot(inertiaAxisTransform.GetRotation());
ndBodyKinematic body;
const ndMatrix matrix(ToNewtonMatrix(m_globalTransform));
body.SetMatrix(matrix);
ndSharedPtr<ndShapeInstance> shape(CreateCollision(matrix));

FTransform tranform;
tranform.SetRotation(FQuat(Inertia.PrincipalInertiaAxis));
const ndMatrix shapeMatrix(ToNewtonMatrix(tranform));
shape->SetLocalMatrix(shapeMatrix * shape->GetLocalMatrix());

bool fullInertia = ndAbs(Inertia.PrincipalInertiaAxis.Pitch) > 0.1f;
fullInertia = fullInertia || (ndAbs(Inertia.PrincipalInertiaAxis.Yaw) > 0.1f);
fullInertia = fullInertia || (ndAbs(Inertia.PrincipalInertiaAxis.Roll) > 0.1f);
body.SetIntrinsicMassMatrix(Mass, **shape, fullInertia);

ndVector centerOfGravity(body.GetCentreOfMass());
centerOfGravity += ndVector(ndFloat32(CenterOfMass.X * UNREAL_INV_UNIT_SYSTEM), ndFloat32(CenterOfMass.Y * UNREAL_INV_UNIT_SYSTEM), ndFloat32(CenterOfMass.Z * UNREAL_INV_UNIT_SYSTEM), ndFloat32(0.0f));
centerOfGravity = matrix.TransformVector(centerOfGravity);

body.SetMatrix(ndGetIdentityMatrix());
ndMatrix inertia(body.CalculateInertiaMatrix());
inertia.EigenVectors();
const ndMatrix axisMatrix(inertia * matrix);
const FTransform axisTranform(ToUnRealTransform(axisMatrix));
const FRotator axisRot(axisTranform.GetRotation());
const FVector axisLoc(centerOfGravity.m_x * UNREAL_UNIT_SYSTEM, centerOfGravity.m_y * UNREAL_UNIT_SYSTEM, centerOfGravity.m_z * UNREAL_UNIT_SYSTEM);
DrawDebugCoordinateSystem(GetWorld(), axisLoc, axisRot, DebugScale * UNREAL_UNIT_SYSTEM, false, timestep);
}

if (ShowCenterOfMass)
if (ShowCenterOfMass && (Mass > 0.0f))
{
ndVector positVolume(ndFloat32(0.0f));
if (m_body)
{
positVolume = m_body->GetCentreOfMass();
}
else
{
ndFixSizeArray<USceneComponent*, 1024> stack;
stack.PushBack(this);

const FTransform tranform(GetComponentToWorld());
const ndMatrix bodyMatrix(ToNewtonMatrix(tranform));

ndFloat32 volume = ndFloat32(0.0f);
while (stack.GetCount())
{
const USceneComponent* const component = stack.Pop();
const UNewtonCollision* const shape = Cast<UNewtonCollision>(component);
if (shape)
{
const ndVector pv(shape->GetVolumePosition(bodyMatrix));
volume += pv.m_w;
positVolume += pv.Scale (pv.m_w);
}

const TArray<TObjectPtr<USceneComponent>>& childrenComp = component->GetAttachChildren();
for (ndInt32 i = childrenComp.Num() - 1; i >= 0; --i)
{
stack.PushBack(childrenComp[i].Get());
}
}
volume = ndMax (volume, ndFloat32(1.0e-3f));
positVolume = positVolume.Scale(ndFloat32(1.0f) / volume);

const ndVector centerOfMass(
ndFloat32(CenterOfMass.X * UNREAL_INV_UNIT_SYSTEM),
ndFloat32(CenterOfMass.Y * UNREAL_INV_UNIT_SYSTEM),
ndFloat32(CenterOfMass.Z * UNREAL_INV_UNIT_SYSTEM),
ndFloat32(0.0f));
positVolume += centerOfMass;
positVolume.m_w = ndFloat32(1.0f);
}

const FTransform tranform(GetComponentToWorld());
const ndMatrix matrix(ToNewtonMatrix(tranform));
positVolume = matrix.TransformVector(positVolume);

ndBodyKinematic body;
ndMatrix matrix(ToNewtonMatrix(m_globalTransform));
body.SetMatrix(matrix);
ndSharedPtr<ndShapeInstance> shape(CreateCollision(matrix));

//FTransform tranform;
//tranform.SetRotation(FQuat(Inertia.PrincipalInertiaAxis));
//const ndMatrix shapeMatrix(ToNewtonMatrix(tranform));
//shape->SetLocalMatrix(shapeMatrix * shape->GetLocalMatrix());

bool fullInertia = ndAbs(Inertia.PrincipalInertiaAxis.Pitch) > 0.1f;
fullInertia = fullInertia || (ndAbs(Inertia.PrincipalInertiaAxis.Yaw) > 0.1f);
fullInertia = fullInertia || (ndAbs(Inertia.PrincipalInertiaAxis.Roll) > 0.1f);

body.SetIntrinsicMassMatrix(Mass, **shape, fullInertia);
ndVector centerOfGravity(body.GetCentreOfMass());
centerOfGravity += ndVector(ndFloat32(CenterOfMass.X * UNREAL_INV_UNIT_SYSTEM), ndFloat32(CenterOfMass.Y * UNREAL_INV_UNIT_SYSTEM), ndFloat32(CenterOfMass.Z * UNREAL_INV_UNIT_SYSTEM), ndFloat32(0.0f));
centerOfGravity = matrix.TransformVector(centerOfGravity);

const FTransform tranform(ToUnRealTransform(matrix));
const FRotator axisRot(tranform.GetRotation());
const FVector axisLoc(positVolume.m_x * UNREAL_UNIT_SYSTEM, positVolume.m_y * UNREAL_UNIT_SYSTEM, positVolume.m_z * UNREAL_UNIT_SYSTEM);
const FVector axisLoc(centerOfGravity.m_x * UNREAL_UNIT_SYSTEM, centerOfGravity.m_y * UNREAL_UNIT_SYSTEM, centerOfGravity.m_z * UNREAL_UNIT_SYSTEM);
DrawDebugCoordinateSystem(GetWorld(), axisLoc, axisRot, DebugScale * UNREAL_UNIT_SYSTEM, false, timestep);
}

Expand Down Expand Up @@ -486,13 +404,8 @@ void UNewtonRigidBody::CreateRigidBody(ANewtonWorldActor* const worldActor, bool
m_body = new ndBodyDynamic();
m_body->SetMatrix(matrix);

ndShapeInstance* const shape = CreateCollision(matrix);
m_body->SetCollisionShape(*shape);

if (shape->GetShape()->GetAsShapeSphere())
{
shape->GetShape()->GetAsShapeSphere();
}
ndSharedPtr<ndShapeInstance> shape(CreateCollision(matrix));
m_body->SetCollisionShape(**shape);

FTransform tranform;
tranform.SetRotation(FQuat(Inertia.PrincipalInertiaAxis));
Expand All @@ -502,10 +415,21 @@ void UNewtonRigidBody::CreateRigidBody(ANewtonWorldActor* const worldActor, bool
bool fullInertia = ndAbs(Inertia.PrincipalInertiaAxis.Pitch) > 0.1f;
fullInertia = fullInertia || (ndAbs(Inertia.PrincipalInertiaAxis.Yaw) > 0.1f);
fullInertia = fullInertia || (ndAbs(Inertia.PrincipalInertiaAxis.Roll) > 0.1f);
m_body->SetMassMatrix(Mass, *shape, fullInertia);

m_body->SetAutoSleep(AutoSleepMode && overrideAutoSleep);
m_body->SetNotifyCallback(new NotifyCallback(this, ndVector(ndFloat32(Gravity.X * UNREAL_INV_UNIT_SYSTEM), ndFloat32(Gravity.Y * UNREAL_INV_UNIT_SYSTEM), ndFloat32(Gravity.Z * UNREAL_INV_UNIT_SYSTEM), ndFloat32(0.0f))));

// Unreal meshes tend to have the origin at the zero value in the z direction,
// this causes SetMassMatrix to generate an skew inertia because of the
// perpendicular axis theorem central.
// when in reality the com is a the geometric center
// and the inertia is relative to that point.
// SetIntrinsicMassMatrix does that.
m_body->SetIntrinsicMassMatrix(Mass, **shape, fullInertia);
ndVector centerOfGravity(m_body->GetCentreOfMass());
centerOfGravity += ndVector(ndFloat32(CenterOfMass.X * UNREAL_INV_UNIT_SYSTEM), ndFloat32(CenterOfMass.Y * UNREAL_INV_UNIT_SYSTEM), ndFloat32(CenterOfMass.Z * UNREAL_INV_UNIT_SYSTEM), ndFloat32(0.0f));
m_body->SetCentreOfMass(centerOfGravity);

m_body->SetLinearDamping(LinearDamp);
m_body->SetAngularDamping(ndVector(AngularDamp));

Expand All @@ -514,13 +438,8 @@ void UNewtonRigidBody::CreateRigidBody(ANewtonWorldActor* const worldActor, bool
m_body->SetOmega(matrix.RotateVector(omega));
m_body->SetVelocity(matrix.RotateVector(veloc));

ndVector centerOfGravity(m_body->GetCentreOfMass());
centerOfGravity += ndVector(ndFloat32(CenterOfMass.X * UNREAL_INV_UNIT_SYSTEM), ndFloat32(CenterOfMass.Y * UNREAL_INV_UNIT_SYSTEM), ndFloat32(CenterOfMass.Z * UNREAL_INV_UNIT_SYSTEM), ndFloat32(0.0f));
m_body->SetCentreOfMass(centerOfGravity);

ndWorld* world = m_newtonWorld->GetNewtonWorld();
ndWorld* const world = m_newtonWorld->GetNewtonWorld();
world->AddBody(m_body);
delete shape;

AActor* const actor = GetOwner();
m_sleeping = false;
Expand Down Expand Up @@ -598,11 +517,28 @@ void UNewtonRigidBody::ApplyPropertyChanges()
m_localScale = m_localTransform.GetScale3D();
m_globalScale = m_globalTransform.GetScale3D();

const ndMatrix inertiaMatrix(CalculateInertiaMatrix());
//float scale = UNREAL_UNIT_SYSTEM * UNREAL_UNIT_SYSTEM * Mass;
//show it in MKS units
//(not in centimeters because it is usually too big number)
float scale = Mass;
const FVector inertia(inertiaMatrix[0][0] * scale, inertiaMatrix[1][1] * scale, inertiaMatrix[2][2] * scale);
Inertia.PrincipalInertia = inertia * Inertia.PrincipalInertiaScaler;
Inertia.PrincipalInertia = FVector(0.0f, 0.0f, 0.0f);
if (Mass > 0.0f)
{
ndBodyKinematic body;
const ndMatrix matrix(ToNewtonMatrix(m_globalTransform));
body.SetMatrix(matrix);
ndSharedPtr<ndShapeInstance> shape(CreateCollision(matrix));

FTransform tranform;
tranform.SetRotation(FQuat(Inertia.PrincipalInertiaAxis));
const ndMatrix shapeMatrix(ToNewtonMatrix(tranform));
shape->SetLocalMatrix(shapeMatrix * shape->GetLocalMatrix());

bool fullInertia = ndAbs(Inertia.PrincipalInertiaAxis.Pitch) > 0.1f;
fullInertia = fullInertia || (ndAbs(Inertia.PrincipalInertiaAxis.Yaw) > 0.1f);
fullInertia = fullInertia || (ndAbs(Inertia.PrincipalInertiaAxis.Roll) > 0.1f);

body.SetIntrinsicMassMatrix(Mass, **shape, fullInertia);
//ndVector centerOfGravity(body->GetCentreOfMass());
//centerOfGravity += ndVector(ndFloat32(CenterOfMass.X * UNREAL_INV_UNIT_SYSTEM), ndFloat32(CenterOfMass.Y * UNREAL_INV_UNIT_SYSTEM), ndFloat32(CenterOfMass.Z * UNREAL_INV_UNIT_SYSTEM), ndFloat32(0.0f));
ndVector massMatrix(body.GetMassMatrix());
ndFloat32 scale2 = UNREAL_UNIT_SYSTEM * UNREAL_UNIT_SYSTEM;
Inertia.PrincipalInertia = FVector(massMatrix.m_x * scale2, massMatrix.m_y * scale2, massMatrix.m_z * scale2);
}
}
1 change: 0 additions & 1 deletion .../applications/toolsAndWrapers/newtonUnreal/newton/Source/newton/Private/NewtonRigidBody.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,6 @@ class UNewtonRigidBody : public USceneComponent
virtual void ClearDebug();
virtual void ActivateDebug();
virtual void ApplyPropertyChanges();
ndMatrix CalculateInertiaMatrix() const;

virtual ndShapeInstance* CreateCollision(const ndMatrix& bodyMatrix) const;

Expand Down

0 comments on commit 9174373

Please sign in to comment.