Skip to content

Commit

Permalink
fixed but in inertia , also adding a tippetop
Browse files Browse the repository at this point in the history
  • Loading branch information
@JulioJerez
JulioJerez committed Nov 1, 2024
1 parent 9174373 commit dfca518
Show file tree
Hide file tree
Showing 3 changed files with 50 additions and 18 deletions.
Binary file added newton-4.00/applications/media/tippeTop.fbx
View file
Binary file not shown.
48 changes: 37 additions & 11 deletions ...pplications/toolsAndWrapers/newtonUnreal/newton/Source/newton/Private/NewtonRigidBody.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -271,10 +271,19 @@ void UNewtonRigidBody::DrawGizmo(float timestep)
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 ndVector massMatrix(body.GetMassMatrix());
const ndMatrix hygenAxis(fullInertia ? body.GetPrincipalAxis() : ndGetIdentityMatrix());
ndMatrix diagonal(ndGetIdentityMatrix());
diagonal[0][0] = massMatrix[0] * Inertia.PrincipalInertiaScaler.X;
diagonal[1][1] = massMatrix[1] * Inertia.PrincipalInertiaScaler.Y;
diagonal[2][2] = massMatrix[2] * Inertia.PrincipalInertiaScaler.Z;
const ndMatrix fullScaledIntertia(hygenAxis.OrthoInverse() * diagonal * hygenAxis);
body.SetMassMatrix(Mass, fullScaledIntertia);

body.SetMatrix(ndGetIdentityMatrix());
ndMatrix inertia(body.CalculateInertiaMatrix());
inertia.EigenVectors();
const ndMatrix axisMatrix(inertia * matrix);
ndMatrix inertiaMatrix(body.CalculateInertiaMatrix());
inertiaMatrix.EigenVectors();
const ndMatrix axisMatrix(inertiaMatrix * 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);
Expand Down Expand Up @@ -415,21 +424,30 @@ 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->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);

const ndVector massMatrix(m_body->GetMassMatrix());
const ndMatrix hygenAxis(fullInertia ? m_body->GetPrincipalAxis() : ndGetIdentityMatrix());
ndMatrix diagonal(ndGetIdentityMatrix());
diagonal[0][0] = massMatrix[0] * Inertia.PrincipalInertiaScaler.X;
diagonal[1][1] = massMatrix[1] * Inertia.PrincipalInertiaScaler.Y;
diagonal[2][2] = massMatrix[2] * Inertia.PrincipalInertiaScaler.Z;
const ndMatrix fullScaledIntertia(hygenAxis.OrthoInverse() * diagonal * hygenAxis);
m_body->SetMassMatrix(Mass, fullScaledIntertia);

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

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

Expand Down Expand Up @@ -535,10 +553,18 @@ void UNewtonRigidBody::ApplyPropertyChanges()
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());

const ndVector massMatrix(body.GetMassMatrix());
const ndMatrix hygenAxis(fullInertia ? body.GetPrincipalAxis() : ndGetIdentityMatrix());
ndMatrix diagonal(ndGetIdentityMatrix());
diagonal[0][0] = massMatrix[0] * Inertia.PrincipalInertiaScaler.X;
diagonal[1][1] = massMatrix[1] * Inertia.PrincipalInertiaScaler.Y;
diagonal[2][2] = massMatrix[2] * Inertia.PrincipalInertiaScaler.Z;
const ndMatrix fullScaledIntertia(hygenAxis.OrthoInverse() * diagonal * hygenAxis);
body.SetMassMatrix(Mass, fullScaledIntertia);

const ndVector scaledMassMatrix(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);
Inertia.PrincipalInertia = FVector(scaledMassMatrix.m_x * scale2, scaledMassMatrix.m_y * scale2, scaledMassMatrix.m_z * scale2);
}
}
20 changes: 13 additions & 7 deletions newton-4.00/sdk/dCollision/ndShapeConvex.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,8 +67,10 @@ void ndShapeConvex::DebugShape(const ndMatrix& matrix, ndShapeDebugNotify& debug
ndAssert(m_edgeCount < D_MAX_EDGE_COUNT);
ndAssert(m_vertexCount < D_MAX_EDGE_COUNT);

memset(mark, 0, sizeof(mark));
memset(edgeType, ndShapeDebugNotify::m_shared, sizeof(edgeType));
//memset(mark, 0, sizeof(mark));
ndMemSet(mark, ndInt8(0), D_MAX_EDGE_COUNT);
//memset(edgeType, ndShapeDebugNotify::m_shared, sizeof(edgeType));
ndMemSet(edgeType, ndShapeDebugNotify::m_shared, D_MAX_EDGE_COUNT);
matrix.TransformTriplex(&tmp[0].m_x, sizeof(ndVector), &m_vertex[0].m_x, sizeof(ndVector), m_vertexCount);
for (ndInt32 i = 0; i < m_edgeCount; ++i)
{
Expand All @@ -94,13 +96,15 @@ void ndShapeConvex::SetVolumeAndCG()
{
ndVector faceVertex[D_MAX_EDGE_COUNT];
ndInt8* const edgeMarks = ndAlloca(ndInt8, m_edgeCount + 32);
memset(&edgeMarks[0], 0, sizeof(ndInt8) * m_edgeCount);
//memset(&edgeMarks[0], 0, sizeof(ndInt8) * m_edgeCount);
ndMemSet(edgeMarks, ndInt8(0), m_edgeCount);

ndPolyhedraMassProperties localData;
for (ndInt32 i = 0; i < m_edgeCount; ++i)
{
ndConvexSimplexEdge* const face = &m_simplex[i];
if (!edgeMarks[i]) {
if (!edgeMarks[i])
{
ndConvexSimplexEdge* edge = face;
ndInt32 count = 0;
do
Expand Down Expand Up @@ -194,7 +198,7 @@ ndMatrix ndShapeConvex::CalculateInertiaAndCenterOfMass(const ndMatrix& alignMat
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;
implicitTest = 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)) &&
Expand Down Expand Up @@ -298,7 +302,8 @@ ndVector ndShapeConvex::SupportVertex(const ndVector& dir) const
ndAssert(ndAbs(dir.DotProduct(dir).GetScalar() - ndFloat32(1.0f)) < ndFloat32(1.0e-3f));

ndInt16 cache[16];
memset(cache, -1, sizeof(cache));
//memset(cache, -1, sizeof(cache));
ndMemSet(cache, ndInt16(-1), 16);
ndConvexSimplexEdge* edge = &m_simplex[0];

ndInt32 index = edge->m_vertex;
Expand Down Expand Up @@ -865,7 +870,8 @@ ndVector ndShapeConvex::CalculateVolumeIntegral(const ndPlane& plane) const
ndConvexSimplexEdge* capEdge = nullptr;

ndVector cg(ndVector::m_zero);
memset(mark, 0, m_edgeCount);
//memset(mark, 0, m_edgeCount);
ndMemSet(mark, ndInt8(0), m_edgeCount);
for (ndInt32 i = 0; i < m_edgeCount; ++i)
{
if (!mark[i])
Expand Down

0 comments on commit dfca518

Please sign in to comment.