private RigidBody CreateRigidBody(GameObject gameObject, Link link)
{
var rb = GetOrCreateComponent <RigidBody>(gameObject);
GetOrCreateComponent <ElementComponent>(gameObject).SetElement(link);
if (link.IsStatic)
{
rb.MotionControl = agx.RigidBody.MotionControl.STATIC;
return(rb);
}
// Note: When <inertial> isn't defined the collision shapes defines
// the mass properties.
// <inertial> defined with required <mass> and <inertia>. Create
// a native rigid body with the given properties and read the
// values back to our rigid body component.
if (link.Inertial != null)
{
var native = new agx.RigidBody();
native.getMassProperties().setMass(link.Inertial.Mass, false);
// Inertia tensor is given in the inertia frame. The rotation of the
// CM frame can't be applied to the CM frame so we transform the inertia
// CM frame and rotate the game object.
var rotationMatrix = link.Inertial.Rpy.RadEulerToRotationMatrix();
var inertia3x3 = (agx.Matrix3x3)link.Inertial.Inertia;
inertia3x3 = rotationMatrix.transpose().Multiply(inertia3x3).Multiply(rotationMatrix);
native.getMassProperties().setInertiaTensor(new agx.SPDMatrix3x3(inertia3x3));
native.getCmFrame().setLocalTranslate(link.Inertial.Xyz.ToVec3());
rb.RestoreLocalDataFrom(native);
}
return(rb);
}
/// <summary>
/// Reads values from native instance.
/// </summary>
/// <param name="native">Source native instance.</param>
public void RestoreLocalDataFrom(agx.RigidBody native)
{
Mass.UserValue = Convert.ToSingle(native.getMassProperties().getMass());
var nativeInertia = native.getMassProperties().getInertiaTensor();
InertiaDiagonal.UserValue = nativeInertia.getDiagonal().ToVector3();
InertiaOffDiagonal.UserValue = GetNativeOffDiagonal(nativeInertia).ToVector3();
CenterOfMassOffset.UserValue = native.getCmFrame().getLocalTranslate().ToHandedVector3();
Mass.UseDefault = false;
InertiaDiagonal.UseDefault = false;
InertiaOffDiagonal.UseDefault = false;
CenterOfMassOffset.UseDefault = false;
}
/// <summary>
/// Callback from RigidBody when mass properties has been calculated for a native instance.
/// </summary>
/// <param name="nativeRb">Native rigid body instance.</param>
public void SetDefaultCalculated(agx.RigidBody nativeRb)
{
if (nativeRb == null)
{
return;
}
Mass.DefaultValue = Convert.ToSingle(nativeRb.getMassProperties().getMass());
CenterOfMassOffset.DefaultValue = nativeRb.getCmFrame().getLocalTranslate().ToHandedVector3();
float inertiaScale = 1.0f;
if (!Mass.UseDefault)
{
inertiaScale = Mass.UserValue / Mass.DefaultValue;
}
InertiaDiagonal.DefaultValue = inertiaScale * nativeRb.getMassProperties().getPrincipalInertiae().ToVector3();
InertiaOffDiagonal.DefaultValue = inertiaScale * GetNativeOffDiagonal(nativeRb.getMassProperties().getInertiaTensor()).ToVector3();
}