// Creates a new rigid body and adds it to the simulation.
private void AddBody(string name, Pose pose, Shape shape, MotionType motionType)
{
var rigidBody = new RigidBody(shape)
{
Name = name,
Pose = pose,
MotionType = motionType,
};
simulation.RigidBodies.Add(rigidBody);
}
//--------------------------------------------------------------
#region Creation & Cleanup
//--------------------------------------------------------------
private void InitializeGeometricObject()
{
_shape = Shape.Infinite;
_pose = Pose.Identity;
}
/// <inheritdoc/>
protected override void CloneCore(Shape sourceShape)
{
}
/// <inheritdoc/>
protected override void CloneCore(Shape sourceShape)
{
var source = (ConvexHullOfPoints)sourceShape;
int numberOfPoints = source.Points.Count;
for (int i = 0; i < numberOfPoints; i++)
_points.Add(source._points[i]);
}
public CollisionAlgorithm this[Shape shapeA, Shape shapeB]
{
get
{
if (shapeA == null)
throw new ArgumentNullException("shapeA");
if (shapeB == null)
throw new ArgumentNullException("shapeB");
return this[shapeA.GetType(), shapeB.GetType()];
}
set
{
if (shapeA == null)
throw new ArgumentNullException("shapeA");
if (shapeB == null)
throw new ArgumentNullException("shapeB");
this[shapeA.GetType(), shapeB.GetType()] = value;
}
}
// Adds a game object and adds a collision object to the collision domain.
private static void AddObject(string name, Pose pose, Shape shape, CollisionDomain collisionDomain)
{
// Create game object.
var geometricObject = new GeometricObject(shape, pose);
// Create collision object.
var collisionObject = new CollisionObject(geometricObject)
{
CollisionGroup = 1,
};
// Add collision object to collision domain.
collisionDomain.CollisionObjects.Add(collisionObject);
}
/// <summary>
/// Initializes a new instance of the <see cref="GeometricObject"/> class with a shape.
/// </summary>
/// <param name="shape">
/// The shape (must not be <see langword="null"/>). See property <see cref="Shape"/> for more
/// details.
/// </param>
/// <exception cref="ArgumentNullException">
/// <paramref name="shape"/> is <see langword="null"/>.
/// </exception>
public GeometricObject(Shape shape)
{
if (shape == null)
throw new ArgumentNullException("shape");
_shape = shape;
_shape.Changed += OnShapeChanged;
_scale = Vector3F.One;
_pose = Pose.Identity;
}
/// <inheritdoc/>
protected override void CloneCore(Shape sourceShape)
{
var source = (HeightField)sourceShape;
_samples = (float[])source.Samples.Clone();
if (source._array != null)
_array = (float[,])source._array.Clone();
_numberOfSamplesX = source._numberOfSamplesX;
_numberOfSamplesZ = source._numberOfSamplesZ;
_depth = source.Depth;
_originX = source.OriginX;
_originZ = source.OriginZ;
_widthX = source.WidthX;
_widthZ = source.WidthZ;
}
// Creates a new rigid body and adds it to the simulation.
private static RigidBody AddBody(Simulation simulation, string name, Pose pose, Shape shape, MotionType motionType)
{
var rigidBody = new RigidBody(shape)
{
Name = name,
Pose = pose,
MotionType = motionType,
};
simulation.RigidBodies.Add(rigidBody);
return rigidBody;
}
//--------------------------------------------------------------
/// <summary>
/// Initializes a new instance of the <see cref="WaterNode" /> class.
/// </summary>
/// <param name="water">The water.</param>
/// <param name="volume">
/// The water volume. Can be <see langword="null"/>, see <see cref="Volume"/>.
/// </param>
/// <exception cref="ArgumentNullException">
/// <paramref name="water"/> is <see langword="null"/>.
/// </exception>
public WaterNode(Water water, Shape volume)
{
if (water == null)
throw new ArgumentNullException("water");
_water = water;
Volume = volume;
// Normally, Volume setter automatically calls Update() - but not when volume is null.
if (Volume == null)
Update(false);
// The IsRenderable flag needs to be set to indicate that the scene node should
// be handled during rendering.
IsRenderable = true;
EnableUnderwaterEffect = true;
//MaxDepth = 100;
}
protected override void Store(Shape wrappedShape)
{
_shapePositioner.WrappedGeometricObject.Shape = wrappedShape;
}
public MyGeometricObject(Shape shape, Pose pose)
: base(shape, pose)
{
}
protected override void CloneCore(Light source)
{
// Clone Light properties.
base.CloneCore(source);
// Clone AmbientLight properties.
var sourceTyped = (AmbientLight)source;
Color = sourceTyped.Color;
Intensity = sourceTyped.Intensity;
HdrScale = sourceTyped.HdrScale;
HemisphericAttenuation = sourceTyped.HemisphericAttenuation;
Shape = source.Shape.Clone();
}
/// <inheritdoc/>
protected override void CloneCore(Shape sourceShape)
{
var source = (ConeShape)sourceShape;
Radius = source.Radius;
Height = source.Height;
}
/// <inheritdoc/>
protected override void CloneCore(Shape sourceShape)
{
var source = (ConvexPolyhedron)sourceShape;
_vertices = source._vertices;
_aabbLocal = source._aabbLocal;
_innerPoint = source._innerPoint;
_directionLookupTable = source._directionLookupTable;
_vertexAdjacency = source._vertexAdjacency;
}
/// <inheritdoc/>
protected override void CloneCore(Shape sourceShape)
{
var source = (ConvexHullOfShapes)sourceShape;
foreach (var geometry in source.Children)
Children.Add((IGeometricObject)geometry.Clone());
}
/// <inheritdoc/>
protected override void CloneCore(Shape sourceShape)
{
var source = (PlaneShape)sourceShape;
_normal = source.Normal;
_distanceFromOrigin = source.DistanceFromOrigin;
}
/// <inheritdoc/>
protected override void CloneCore(Shape sourceShape)
{
var source = (RectangleShape)sourceShape;
_widthX = source.WidthX;
_widthY = source.WidthY;
}
//--------------------------------------------------------------
#region Creation & Cleanup
//--------------------------------------------------------------
/// <overloads>
/// <summary>
/// Initializes a new instance of the <see cref="GeometricObject"/> class.
/// </summary>
/// </overloads>
///
/// <summary>
/// Initializes a new instance of the <see cref="GeometricObject"/> class.
/// </summary>
public GeometricObject()
{
_shape = Shape.Empty;
_scale = Vector3F.One;
_pose = Pose.Identity;
}
/// <summary>
/// Resets the collision object. (For internal use only.)
/// </summary>
internal void ResetInternal()
{
Changed = true;
CollisionGroup = 0;
_domain = null;
Enabled = true;
_geometricObject = null;
_type = CollisionObjectType.Default;
_shapeType = ShapeType.Default;
_shape = null;
ShapeTypeChanged = true;
}
/// <summary>
/// Initializes a new instance of the <see cref="GeometricObject"/> class with shape, scale and
/// pose (position and orientation).
/// </summary>
/// <param name="shape">
/// The shape (must not be <see langword="null"/>). See property <see cref="Shape"/> for more
/// details.
/// </param>
/// <param name="scale">The scale.</param>
/// <param name="pose">The pose (position and orientation).</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="shape"/> is <see langword="null"/>.
/// </exception>
public GeometricObject(Shape shape, Vector3F scale, Pose pose)
{
if (shape == null)
throw new ArgumentNullException("shape");
_shape = shape;
_shape.Changed += OnShapeChanged;
_scale = scale;
_pose = pose;
}
// The following methods are used internally by the collision detection to make direct
// changes to a CollisionObject during collision checks.
/// <summary>
/// Copies the data from the specified <see cref="CollisionObject"/> and sets the specified
/// <see cref="IGeometricObject"/>. (For internal use only.)
/// </summary>
/// <param name="collisionObject">The collision object.</param>
/// <param name="geometricObject">The geometric object.</param>
internal void SetInternal(CollisionObject collisionObject, IGeometricObject geometricObject)
{
Changed = collisionObject.Changed;
CollisionGroup = collisionObject.CollisionGroup;
_domain = collisionObject._domain;
Enabled = collisionObject.Enabled;
_geometricObject = geometricObject;
_type = collisionObject._type;
_shapeType = collisionObject._shapeType;
_shape = geometricObject.Shape;
ShapeTypeChanged = collisionObject.ShapeTypeChanged;
}
/// <inheritdoc/>
protected override void CloneCore(Shape sourceShape)
{
var source = (BoxShape)sourceShape;
_widthX = source.WidthX;
_widthY = source.WidthY;
_widthZ = source.WidthZ;
}
/// <summary>
/// Called when the shape was changed.
/// </summary>
/// <param name="sender">The sender.</param>
/// <param name="eventArgs">
/// The <see cref="ShapeChangedEventArgs"/> instance containing the event data.
/// </param>
private void OnShapeChanged(object sender, ShapeChangedEventArgs eventArgs)
{
Changed = true;
var shape = (_geometricObject != null) ? _geometricObject.Shape : null;
// Check if shape type was changed (which invalidates the cached collision algos).
// Instead of comparing the types, we simply compare the instances.
ShapeTypeChanged = ShapeTypeChanged || (_shape != shape);
// Remember current shape.
_shape = shape;
RayShape ray = shape as RayShape;
if (ray != null)
_shapeType = ray.StopsAtFirstHit ? ShapeType.RayThatStopsAtFirstHit : ShapeType.Ray;
else
_shapeType = ShapeType.Default;
if (_domain == null)
return;
Debug.Assert(_domain != null);
// Clear existing contact info only.
int feature = eventArgs.Feature;
if (feature == -1)
{
// Invalidate all contacts that contain this collision object.
foreach (var contactSet in _domain.ContactSets.GetContacts(this))
{
foreach (var contact in contactSet)
contact.Recycle();
contactSet.Clear();
contactSet.IsValid = false;
}
return;
}
Debug.Assert(feature >= 0);
// Remove only the contacts of the given feature.
foreach (var contactSet in _domain.ContactSets.GetContacts(this))
{
if (contactSet.ObjectA == this)
{
for (int i = contactSet.Count - 1; i >= 0; i--)
{
Contact contact = contactSet[i];
if (contact.FeatureA == -1 || contact.FeatureA == feature)
{
contactSet.RemoveAt(i);
contact.Recycle();
}
}
}
else
{
Debug.Assert(contactSet.ObjectB == this);
for (int i = contactSet.Count - 1; i >= 0; i--)
{
Contact contact = contactSet[i];
if (contact.FeatureB == -1 || contact.FeatureB == feature)
{
contactSet.RemoveAt(i);
contact.Recycle();
}
}
}
contactSet.IsValid = false;
}
}
/// <inheritdoc/>
protected override void CloneCore(Shape sourceShape)
{
var source = (CapsuleShape)sourceShape;
_radius = source.Radius;
_height = source.Height;
}
internal static void GetMass(Shape shape, Vector3F scale, float densityOrMass, bool isDensity, float relativeDistanceThreshold, int iterationLimit,
out float mass, out Vector3F centerOfMass, out Matrix33F inertia)
{
if (shape == null)
throw new ArgumentNullException("shape");
if (densityOrMass <= 0)
throw new ArgumentOutOfRangeException("densityOrMass", "The density or mass must be greater than 0.");
if (relativeDistanceThreshold < 0)
throw new ArgumentOutOfRangeException("relativeDistanceThreshold", "The relative distance threshold must not be negative.");
mass = 0;
centerOfMass = Vector3F.Zero;
inertia = Matrix33F.Zero;
// Note: We support all shape types of DigitalRune Geometry.
// To support user-defined shapes we could add an interface IMassSource which can be
// implemented by shapes. In the else-case below we can check whether the shape implements
// the interface.
if (shape is EmptyShape)
{
return;
}
else if (shape is InfiniteShape)
{
mass = float.PositiveInfinity;
inertia = Matrix33F.CreateScale(float.PositiveInfinity);
}
else if (shape is BoxShape)
{
GetMass((BoxShape)shape, scale, densityOrMass, isDensity, out mass, out inertia);
}
else if (shape is CapsuleShape)
{
GetMass((CapsuleShape)shape, scale, densityOrMass, isDensity, out mass, out inertia);
}
else if (shape is ConeShape)
{
GetMass((ConeShape)shape, scale, densityOrMass, isDensity, out mass, out centerOfMass, out inertia);
}
else if (shape is CylinderShape)
{
GetMass((CylinderShape)shape, scale, densityOrMass, isDensity, out mass, out inertia);
}
else if (shape is ScaledConvexShape)
{
var scaledConvex = (ScaledConvexShape)shape;
GetMass(scaledConvex.Shape, scale * scaledConvex.Scale, densityOrMass, isDensity, relativeDistanceThreshold, iterationLimit, out mass, out centerOfMass, out inertia);
}
else if (shape is SphereShape)
{
GetMass((SphereShape)shape, scale, densityOrMass, isDensity, out mass, out inertia);
}
else if (shape is TransformedShape)
{
var transformed = (TransformedShape)shape;
// Call GetMass for the contained GeometricObject.
GetMass(transformed.Child, scale, densityOrMass, isDensity, relativeDistanceThreshold, iterationLimit, out mass, out centerOfMass, out inertia);
}
else if (shape is HeightField)
{
// Height fields should always be static. Therefore, they we can treat them as having
// infinite or zero mass.
return;
}
else if (shape is CompositeShape)
{
var composite = (CompositeShape)shape;
float density = (isDensity) ? densityOrMass : 1;
foreach (var child in composite.Children)
{
// Call GetMass for the child geometric object.
float childMass;
Vector3F childCenterOfMass;
Matrix33F childInertia;
GetMass(child, scale, density, true, relativeDistanceThreshold, iterationLimit, out childMass, out childCenterOfMass, out childInertia);
// Add child mass to total mass.
mass = mass + childMass;
// Add child inertia to total inertia and consider the translation.
inertia += GetTranslatedMassInertia(childMass, childInertia, childCenterOfMass);
// Add weighted centerOfMass.
centerOfMass = centerOfMass + childCenterOfMass * childMass;
}
// centerOfMass must be divided by total mass because child center of mass were weighted
// with the child masses.
centerOfMass /= mass;
// Make inertia relative to center of mass.
inertia = GetUntranslatedMassInertia(mass, inertia, centerOfMass);
if (!isDensity)
{
// Yet, we have not computed the correct total mass. We have to adjust the total mass to
// be equal to the given target mass.
AdjustMass(densityOrMass, ref mass, ref inertia);
}
}
else if (iterationLimit <= 0)
{
// We do not have a special formula for this kind of shape and iteration limit is 0 or less.
// --> Use mass properties of AABB.
var aabb = shape.GetAabb(scale, Pose.Identity);
var extent = aabb.Extent;
centerOfMass = aabb.Center;
GetMass(extent, densityOrMass, isDensity, out mass, out inertia);
}
else
{
// We do not have a special formula for this kind of shape.
// --> General polyhedron mass from triangle mesh.
var mesh = shape.GetMesh(relativeDistanceThreshold, iterationLimit);
mesh.Transform(Matrix44F.CreateScale(scale));
GetMass(mesh, out mass, out centerOfMass, out inertia);
// Mass was computed for density = 1. --> Scale mass.
if (isDensity)
{
var volume = mesh.GetVolume();
var targetMass = volume * densityOrMass;
AdjustMass(targetMass, ref mass, ref inertia);
}
else
{
AdjustMass(densityOrMass, ref mass, ref inertia);
}
if (Numeric.IsLessOrEqual(mass, 0))
{
// If the mass is not valid, we fall back to the AABB mass.
// This can happen for non-closed meshes that have a "negative" volume.
GetMass(shape, scale, densityOrMass, isDensity, relativeDistanceThreshold, -1, out mass, out centerOfMass, out inertia);
return;
}
}
}
/// <inheritdoc/>
protected override void CloneCore(Shape sourceShape)
{
var source = (RayShape)sourceShape;
_origin = source.Origin;
_direction = source.Direction;
_length = source.Length;
_stopsAtFirstHit = source.StopsAtFirstHit;
}
/// <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>
public RigidBody(Shape shape)
: this(shape, null, null)
{
}
/// <inheritdoc/>
protected override void CloneCore(Shape sourceShape)
{
var source = (CircleShape)sourceShape;
_radius = source.Radius;
}
/// <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;
}
