/// <summary>
/// Initializes a new instance of the <see cref="RigidBody"/> class.
/// </summary>
/// <param name="shape">
/// The shape. Can be <see langword="null"/> to use the default <see cref="Shape"/>.
/// </param>
/// <param name="massFrame">
/// The mass frame. Can be <see langword="null"/> in which case the mass properties for a
/// density of 1000 are used.
/// </param>
/// <param name="material">
/// The material. Can be <see langword="null"/> to use the default <see cref="Material"/>.
/// </param>
public RigidBody(Shape shape, MassFrame massFrame, IMaterial material)
{
AutoUpdateMass = true;
IslandId = -1;
Name = "Unnamed";
_pose = Pose.Identity;
_shape = shape ?? new BoxShape(1, 1, 1);
_shape.Changed += OnShapeChanged;
_scale = Vector3F.One;
Material = material ?? new UniformMaterial();
if (massFrame != null)
{
MassFrame = massFrame;
}
else
{
UpdateMassFrame();
}
CollisionResponseEnabled = true;
MotionType = MotionType.Dynamic;
CollisionObject = new CollisionObject(this);
CanSleep = true;
IsSleeping = false;
TimeOfImpact = 1;
}
//--------------------------------------------------------------
#region Creation & Cleanup
//--------------------------------------------------------------
#endregion
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
/// <summary>
/// Creates a new <see cref="MassFrame"/> that is a clone (deep copy) of the current instance.
/// </summary>
/// <returns>
/// A new <see cref="MassFrame"/> that is a clone (deep copy) of the current instance.
/// </returns>
public MassFrame Clone()
{
MassFrame clone = new MassFrame
{
_density = _density,
_inertia = _inertia,
_inertiaInverse = _inertiaInverse,
_mass = _mass,
_massInverse = _massInverse,
_pose = _pose
};
return(clone);
}
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
/// <summary>
/// Updates the mass frame for a new shape and scale.
/// </summary>
private void UpdateMassFrame()
{
if (MassFrame == null)
{
MassFrame = MassFrame.FromShapeAndDensity(Shape, Scale, 1000, 0.01f, 3);
return;
}
if (AutoUpdateMass)
{
// Compute mass for the same density as last time or the same target mass as last time.
if (MassFrame.Density > 0)
{
MassFrame = MassFrame.FromShapeAndDensity(Shape, Scale, MassFrame.Density, 0.01f, 3);
}
else
{
MassFrame = MassFrame.FromShapeAndMass(Shape, Scale, MassFrame.Mass, 0.01f, 3);
}
}
}
/// <summary>
/// Computes the mass properties for the given shape and parameters.
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="scale">The scale.</param>
/// <param name="densityOrMass">The density or target mass value.</param>
/// <param name="isDensity">
/// If set to <see langword="true"/> <paramref name="densityOrMass"/> is interpreted as density;
/// otherwise, <paramref name="densityOrMass"/> is interpreted as the desired target mass.
/// </param>
/// <param name="relativeDistanceThreshold">The relative distance threshold.</param>
/// <param name="iterationLimit">
/// The iteration limit. Can be -1 or 0 to use an approximation.
/// </param>
/// <returns>
/// A new <see cref="MassFrame"/> instance.
/// </returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="shape"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentOutOfRangeException">
/// <paramref name="densityOrMass"/> is negative or 0.
/// </exception>
/// <exception cref="ArgumentOutOfRangeException">
/// <paramref name="relativeDistanceThreshold"/> is negative.
/// </exception>
private static MassFrame ComputeMassProperties(Shape shape, Vector3F scale, float densityOrMass, bool isDensity, float relativeDistanceThreshold, int iterationLimit)
{
if (shape == null)
{
throw new ArgumentNullException("shape");
}
if (densityOrMass <= 0)
{
throw new ArgumentOutOfRangeException("densityOrMass", "Density and mass must be greater than 0.");
}
if (relativeDistanceThreshold < 0)
{
throw new ArgumentOutOfRangeException("relativeDistanceThreshold", "Relative distance threshold must not be negative.");
}
// Call MassHelper to compute mass, COM and inertia matrix (not diagonalized).
float mass;
Vector3F centerOfMass;
Matrix33F inertia;
MassHelper.GetMass(shape, scale, densityOrMass, isDensity, relativeDistanceThreshold, iterationLimit,
out mass, out centerOfMass, out inertia);
// If anything is NaN, we use default values for a static/kinematic body.
if (Numeric.IsNaN(mass) || centerOfMass.IsNaN || inertia.IsNaN)
{
return new MassFrame {
Mass = 0, Inertia = Vector3F.Zero
}
}
;
if (!Numeric.IsZero(inertia.M01) || !Numeric.IsZero(inertia.M02) || !Numeric.IsZero(inertia.M12))
{
// Inertia off-diagonal elements are not 0.
// --> Have to make inertia a diagonal matrix.
Vector3F inertiaDiagonal;
Matrix33F rotation;
MassHelper.DiagonalizeInertia(inertia, out inertiaDiagonal, out rotation);
MassFrame massFrame = new MassFrame
{
Mass = mass,
Inertia = inertiaDiagonal,
Pose = new Pose(centerOfMass, rotation),
Density = isDensity ? densityOrMass : 0
};
return(massFrame);
}
else
{
// Inertia is already a diagonal matrix.
MassFrame massFrame = new MassFrame
{
Mass = mass,
Inertia = new Vector3F(inertia.M00, inertia.M11, inertia.M22),
Pose = new Pose(centerOfMass),
Density = isDensity ? densityOrMass : 0
};
return(massFrame);
}
}
/// <summary>
/// Initializes a new instance of the <see cref="RigidBody"/> class.
/// </summary>
/// <param name="shape">
/// The shape. Can be <see langword="null"/> to use the default <see cref="Shape"/>.
/// </param>
/// <param name="massFrame">
/// The mass frame. Can be <see langword="null"/> in which case the mass properties for a
/// density of 1000 are used.
/// </param>
/// <param name="material">
/// The material. Can be <see langword="null"/> to use the default <see cref="Material"/>.
/// </param>
public RigidBody(Shape shape, MassFrame massFrame, IMaterial material)
{
AutoUpdateMass = true;
IslandId = -1;
Name = "Unnamed";
_pose = Pose.Identity;
_shape = shape ?? new BoxShape(1, 1, 1);
_shape.Changed += OnShapeChanged;
_scale = Vector3F.One;
Material = material ?? new UniformMaterial();
if (massFrame != null)
MassFrame = massFrame;
else
UpdateMassFrame();
CollisionResponseEnabled = true;
MotionType = MotionType.Dynamic;
CollisionObject = new CollisionObject(this);
CanSleep = true;
IsSleeping = false;
TimeOfImpact = 1;
}
/// <summary>
/// Computes the mass properties for the given shape and parameters.
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="scale">The scale.</param>
/// <param name="densityOrMass">The density or target mass value.</param>
/// <param name="isDensity">
/// If set to <see langword="true"/> <paramref name="densityOrMass"/> is interpreted as density;
/// otherwise, <paramref name="densityOrMass"/> is interpreted as the desired target mass.
/// </param>
/// <param name="relativeDistanceThreshold">The relative distance threshold.</param>
/// <param name="iterationLimit">
/// The iteration limit. Can be -1 or 0 to use an approximation.
/// </param>
/// <returns>
/// A new <see cref="MassFrame"/> instance.
/// </returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="shape"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentOutOfRangeException">
/// <paramref name="densityOrMass"/> is negative or 0.
/// </exception>
/// <exception cref="ArgumentOutOfRangeException">
/// <paramref name="relativeDistanceThreshold"/> is negative.
/// </exception>
private static MassFrame ComputeMassProperties(Shape shape, Vector3F scale, float densityOrMass, bool isDensity, float relativeDistanceThreshold, int iterationLimit)
{
if (shape == null)
throw new ArgumentNullException("shape");
if (densityOrMass <= 0)
throw new ArgumentOutOfRangeException("densityOrMass", "Density and mass must be greater than 0.");
if (relativeDistanceThreshold < 0)
throw new ArgumentOutOfRangeException("relativeDistanceThreshold", "Relative distance threshold must not be negative.");
// Call MassHelper to compute mass, COM and inertia matrix (not diagonalized).
float mass;
Vector3F centerOfMass;
Matrix33F inertia;
MassHelper.GetMass(shape, scale, densityOrMass, isDensity, relativeDistanceThreshold, iterationLimit,
out mass, out centerOfMass, out inertia);
// If anything is NaN, we use default values for a static/kinematic body.
if (Numeric.IsNaN(mass) || centerOfMass.IsNaN || inertia.IsNaN)
return new MassFrame { Mass = 0, Inertia = Vector3F.Zero };
if (!Numeric.IsZero(inertia.M01) || !Numeric.IsZero(inertia.M02) || !Numeric.IsZero(inertia.M12))
{
// Inertia off-diagonal elements are not 0.
// --> Have to make inertia a diagonal matrix.
Vector3F inertiaDiagonal;
Matrix33F rotation;
MassHelper.DiagonalizeInertia(inertia, out inertiaDiagonal, out rotation);
MassFrame massFrame = new MassFrame
{
Mass = mass,
Inertia = inertiaDiagonal,
Pose = new Pose(centerOfMass, rotation),
Density = isDensity ? densityOrMass : 0
};
return massFrame;
}
else
{
// Inertia is already a diagonal matrix.
MassFrame massFrame = new MassFrame
{
Mass = mass,
Inertia = new Vector3F(inertia.M00, inertia.M11, inertia.M22),
Pose = new Pose(centerOfMass),
Density = isDensity ? densityOrMass : 0
};
return massFrame;
}
}
//--------------------------------------------------------------
/// <summary>
/// Creates a new <see cref="MassFrame"/> that is a clone (deep copy) of the current instance.
/// </summary>
/// <returns>
/// A new <see cref="MassFrame"/> that is a clone (deep copy) of the current instance.
/// </returns>
public MassFrame Clone()
{
MassFrame clone = new MassFrame
{
_density = _density,
_inertia = _inertia,
_inertiaInverse = _inertiaInverse,
_mass = _mass,
_massInverse = _massInverse,
_pose = _pose
};
return clone;
}
