/// <summary>
/// Implements the + operator.
/// </summary>
/// <param name="a">a.</param>
/// <param name="b">The b.</param>
/// <returns>The result of the operator.</returns>
/// <exception cref="ArgumentException">Masses must both be in local coordinate systems in order to be added together.</exception>
public static Mass operator +(Mass a, Mass b)
{
if (a == null)
{
return(b);
}
if (b == null)
{
return(a);
}
if (!a.IsLocalCoordinateSystem || !b.IsLocalCoordinateSystem)
{
throw new ArgumentException("Masses must both be in local coordinate systems in order to be added together.");
}
MassProperties massA = a.GetMass();
MassProperties massB = b.GetMass();
MassProperties mass = new MassProperties()
{
U1 = massA.U1 + massB.U1,
U2 = massA.U2 + massB.U2,
U3 = massA.U3 + massB.U3,
R1 = massA.R1 + massB.R1,
R2 = massA.R2 + massB.R2,
R3 = massA.R3 + massB.R3
};
return(new Mass(mass));
}
public MaterialGlobalInfos(
AssemblyDocument document,
Material material,
List <ComponentOccurrence> materialOccurrences)
{
MassProperties asmMassProperties = document.ComponentDefinition.MassProperties;
Material = material;
GlobalInfos = new MaterialInfos(material);
BreakDownInfos = new Dictionary <ComponentOccurrence, MaterialInfos>();
string docVolumeUnits = GetDocVolumeUnits(document as Document, false);
string dbVolumeUnits = GetDbVolumeUnits(document as Document, false);
foreach (ComponentOccurrence occurrence in materialOccurrences)
{
MaterialInfos materialInfos = new MaterialInfos(material);
//Compute percentage first, so no need to convert in doc units first
materialInfos.MassPercentage =
occurrence.MassProperties.Mass * 100.0 / asmMassProperties.Mass;
materialInfos.DbMass = occurrence.MassProperties.Mass;
materialInfos.Volume = document.UnitsOfMeasure.ConvertUnits(
occurrence.MassProperties.Volume,
dbVolumeUnits,
docVolumeUnits);
materialInfos.Mass = document.UnitsOfMeasure.ConvertUnits(
occurrence.MassProperties.Mass,
UnitsTypeEnum.kDatabaseMassUnits,
UnitsTypeEnum.kDefaultDisplayMassUnits);
BreakDownInfos.Add(occurrence, materialInfos);
//Add to global, database value
GlobalInfos.DbMass += materialInfos.DbMass;
GlobalInfos.Volume += occurrence.MassProperties.Volume;
GlobalInfos.Mass += occurrence.MassProperties.Mass;
}
//Compute percentage first, so no need to convert in doc units first
GlobalInfos.MassPercentage = GlobalInfos.Mass * 100.0 / asmMassProperties.Mass;
//then convert to doc units
GlobalInfos.Volume = document.UnitsOfMeasure.ConvertUnits(
GlobalInfos.Volume,
dbVolumeUnits,
docVolumeUnits);
GlobalInfos.Mass = document.UnitsOfMeasure.ConvertUnits(
GlobalInfos.Mass,
UnitsTypeEnum.kDatabaseMassUnits,
UnitsTypeEnum.kDefaultDisplayMassUnits);
}
public Entity CreateBody(RenderMesh displayMesh, float3 position, quaternion orientation,
BlobAssetReference <Unity.Physics.Collider> collider,
float3 linearVelocity, float3 angularVelocity, float mass, bool isDynamic)
{
ComponentType[] componentTypes = new ComponentType[isDynamic ? 9 : 6];
componentTypes[0] = typeof(RenderMesh);
componentTypes[1] = typeof(TranslationProxy);
componentTypes[2] = typeof(RotationProxy);
componentTypes[3] = typeof(PhysicsCollider);
componentTypes[4] = typeof(Translation);
componentTypes[5] = typeof(Rotation);
if (isDynamic)
{
componentTypes[6] = typeof(PhysicsVelocity);
componentTypes[7] = typeof(PhysicsMass);
componentTypes[8] = typeof(PhysicsDamping);
}
EntityManager entityManager = EntityManager;
Entity entity = entityManager.CreateEntity(componentTypes);
entityManager.SetName(entity, "randomSphere");
entityManager.SetSharedComponentData(entity, displayMesh);
entityManager.AddComponentData(entity, new LocalToWorld {
});
entityManager.SetComponentData(entity, new Translation {
Value = position
});
entityManager.SetComponentData(entity, new Rotation {
Value = orientation
});
entityManager.SetComponentData(entity, new PhysicsCollider {
Value = collider
});
if (isDynamic)
{
MassProperties massProperties = collider.Value.MassProperties;
entityManager.SetComponentData(entity, PhysicsMass.CreateDynamic(
massProperties, mass));
float3 angularVelocityLocal = math.mul(
math.inverse(massProperties.MassDistribution.Transform.rot),
angularVelocity);
entityManager.SetComponentData(entity, new PhysicsVelocity()
{
Linear = linearVelocity,
Angular = angularVelocityLocal
});
entityManager.SetComponentData(entity, new PhysicsDamping()
{
Linear = 0.01f,
Angular = 0.05f
});
}
return(entity);
}
private PhysicalProperties ExportPhysicalProperties(MassProperties massProperties)
{
var physicalProperties = new PhysicalProperties();
physicalProperties.Mass = massProperties.Mass; // kg -> kg
physicalProperties.Area = massProperties.Area / Math.Pow(100, 2); // cm^2 -> m^2
physicalProperties.Volume = massProperties.Volume / Math.Pow(100, 3); // cm^3 -> m^3
physicalProperties.CenterOfMass = GetVector3DConvertUnits(massProperties.CenterOfMass);
return(physicalProperties);
}
public RigidBodyModel(MassProperties mass, CompiledPart[] parts,
Material[] materials)
{
_mass = mass;
_parts = parts;
_materials = materials;
if (_parts.Length != _materials.Length)
{
throw new ArgumentException("The count of supplied mesh parts and materials do not match.");
}
}
public void TestBoxColliderMassProperties()
{
float3 center = float3.zero;
quaternion orientation = quaternion.identity;
float3 size = new float3(1.0f, 250.0f, 2.0f);
float convexRadius = 0.25f;
var boxCollider = BoxCollider.Create(center, orientation, size, convexRadius);
float3 expectedInertiaTensor = 1.0f / 12.0f * new float3(
size.y * size.y + size.z * size.z,
size.x * size.x + size.z * size.z,
size.y * size.y + size.x * size.x);
MassProperties massProperties = boxCollider.Value.MassProperties;
float3 inertiaTensor = massProperties.MassDistribution.InertiaTensor;
Debug.Log($"Expected Inertia Tensor: {expectedInertiaTensor}, was: {inertiaTensor}");
TestUtils.AreEqual(expectedInertiaTensor, inertiaTensor, 1e-3f);
}
public double[] FindCenterOfMassOffset(ComponentOccurrence oDoc)
{
// Store temporary variables and names
MassProperties oMassProps = oDoc.MassProperties;
double[] c = new double[3];
c[0] = oMassProps.CenterOfMass.X;
c[1] = oMassProps.CenterOfMass.Y;
c[2] = oMassProps.CenterOfMass.Z;
UnitsOfMeasure oUOM = _invApp.ActiveDocument.UnitsOfMeasure;
for (int k = 0; k < 3; k++)
{
c[k] = oUOM.ConvertUnits(c[k], "cm", "m");
}
return(c);
}
public void TestBoxColliderMassProperties()
{
var geometry = new BoxGeometry
{
Center = float3.zero,
Orientation = quaternion.identity,
Size = new float3(1.0f, 250.0f, 2.0f),
BevelRadius = 0.25f
};
var boxCollider = BoxCollider.Create(geometry);
float3 expectedInertiaTensor = 1.0f / 12.0f * new float3(
geometry.Size.y * geometry.Size.y + geometry.Size.z * geometry.Size.z,
geometry.Size.x * geometry.Size.x + geometry.Size.z * geometry.Size.z,
geometry.Size.y * geometry.Size.y + geometry.Size.x * geometry.Size.x);
MassProperties massProperties = boxCollider.Value.MassProperties;
float3 inertiaTensor = massProperties.MassDistribution.InertiaTensor;
TestUtils.AreEqual(expectedInertiaTensor, inertiaTensor, 1e-3f);
}
public ScaledSphere(Game game, float radius, Vector3 color)
{
_model = game.Content.Load <Model>("models/sphere");
_diffuseColor = color;
_meshTransform = Matrix.CreateScale(radius / 0.25f);
this.MassProperties = MassProperties.FromSphere(1f, Vector3.Zero, radius);
this.Skin.Add(new SpherePart(new Sphere(Vector3.Zero, radius)), new Material(0f, 0.5f));
foreach (var mesh in _model.Meshes)
{
foreach (BasicEffect effect in mesh.Effects)
{
effect.EnableDefaultLighting();
effect.AmbientLightColor = Vector3.One * 0.75f;
effect.SpecularColor = Vector3.One;
effect.PreferPerPixelLighting = true;
}
}
}
public Inertial(MassProperties massProperties)
{
mass.value = Math.Round(massProperties.Mass, 4);
// Get mass properties for each link.
double[] iXYZ = new double[6];
massProperties.XYZMomentsOfInertia(out iXYZ[0], out iXYZ[3], out iXYZ[5], out iXYZ[1], out iXYZ[4], out iXYZ[2]); // Ixx, Iyy, Izz, Ixy, Iyz, Ixz -> Ixx, Ixy, Ixz, Iyy, Iyz, Izz
iXYZ = iXYZ.Select(x => Math.Round(Math.Round(x) * 0.0001, 7)).ToArray();
inertiaVector.ixx = iXYZ[0];
inertiaVector.ixy = iXYZ[1];
inertiaVector.ixz = iXYZ[2];
inertiaVector.iyy = iXYZ[3];
inertiaVector.iyz = iXYZ[4];
inertiaVector.izz = iXYZ[5];
InertiaMatrix = new double[, ] {
{ iXYZ[0], iXYZ[1], iXYZ[2] },
{ iXYZ[1], iXYZ[3], iXYZ[4] },
{ iXYZ[2], iXYZ[4], iXYZ[5] }
};
Origin = new Origin();
Origin.XYZ = new double[] { massProperties.CenterOfMass.X * 0.01, massProperties.CenterOfMass.Y * 0.01, massProperties.CenterOfMass.Z * 0.01 };
}
public ScaledCapsule(Game game, float length, float radius, Vector3 color)
{
_model = game.Content.Load <Model>("models/capsule");
_meshTransform = _model.Meshes[0].ParentBone.Transform *
Matrix.CreateScale(new Vector3(radius / 0.25f, radius / 0.25f, length + (radius * 2)));
_diffuseColor = color;
Vector3 dummy, p1 = new Vector3(0f, 0f, length / 2f), p2 = new Vector3(0f, 0f, -length / 2f);
this.MassProperties = MassProperties.FromCapsule(1f, p1, p2, radius, out dummy);
this.Skin.Add(new CapsulePart(new Capsule(p1, p2, radius)), new Material(0f, 0.5f));
foreach (var mesh in _model.Meshes)
{
foreach (BasicEffect effect in mesh.Effects)
{
effect.EnableDefaultLighting();
effect.AmbientLightColor = Vector3.One * 0.75f;
effect.SpecularColor = Vector3.One;
effect.PreferPerPixelLighting = true;
}
}
}
/// <summary>
/// Initializes a new instance of the <see cref="Mass"/> class.
/// </summary>
/// <param name="mass">The mass.</param>
/// <param name="isLocalCoordinateSystem">if set to <c>true</c> [is local coordinate system].</param>
internal Mass(MassProperties mass, bool isLocalCoordinateSystem = true)
{
_mass = mass;
IsLocalCoordinateSystem = isLocalCoordinateSystem;
}
public void CreateScene(int sceneNumber)
{
_physics.Clear();
_markers.Clear();
Room room = new Room(this);
_physics.Add(room);
_physics.Gravity = new Vector3(0f, 0f, -9.8f);
Model cubeModel = this.Content.Load <Model>("models/small_cube");
Model obeliskModel = this.Content.Load <Model>("models/obelisk");
Model sphereModel = this.Content.Load <Model>("models/sphere");
Model capsuleModel = this.Content.Load <Model>("models/capsule");
Model torusModel = this.Content.Load <Model>("models/torus");
Model slabModel = this.Content.Load <Model>("models/slab");
Model triangleModel = this.Content.Load <Model>("models/triangle");
switch (sceneNumber)
{
case 1:
{
for (int i = 0; i < 12; i++)
{
var cube = new SolidThing(this, cubeModel);
cube.SetWorld(new Vector3(0f, 0f, 0.25f + 0.51f * i));
_physics.Add(cube);
}
}
break;
case 2:
{
for (int i = 0; i < 7; i++)
{
for (int j = 0; j < 7 - i; j++)
{
var cube = new SolidThing(this, cubeModel);
cube.SetWorld(new Vector3(0f, 0.501f * j + 0.25f * i, 0.5f + 0.55f * i));
_physics.Add(cube);
}
}
}
break;
case 3:
{
for (int i = 0; i < 6; i++)
{
for (int j = 0; j < 6 - i; j++)
{
var cube = new SolidThing(this, cubeModel);
cube.SetWorld(new Vector3(0f, 2.2f * j + 1f * i - 4.1f, 0.75f * i + 0.25f));
_physics.Add(cube);
}
}
for (int i = 0; i < 6; i++)
{
for (int j = 0; j < 5 - i; j++)
{
var plank = new SolidThing(this, obeliskModel);
plank.SetWorld(new Vector3(0f, 2.2f * j + 1f * i + 1f - 4f, 0.75f * i + 0.65f),
Quaternion.CreateFromAxisAngle(Vector3.UnitZ, MathHelper.ToRadians(90f)));
_physics.Add(plank);
}
}
}
break;
case 4:
{
int size = 9;
#if WINDOWS
#else
size = 4;
#endif
for (int i = 0; i < size; i++)
{
for (int j = 0; j < size - i; j++)
{
for (int k = 0; k < size - i; k++)
{
var sphere = new SolidThing(this, sphereModel);
sphere.SetWorld(new Vector3(
0.501f * j + 0.25f * i,
0.501f * k + 0.25f * i,
0.501f * i + 0.5f
), Quaternion.Identity);
_physics.Add(sphere);
}
}
}
}
break;
case 5:
{
var plank = new SolidThing(this, obeliskModel);
plank.SetWorld(new Vector3(0.0f, 0.0f, 4.0f),
Quaternion.CreateFromAxisAngle(Vector3.UnitY, MathHelper.ToRadians(15f)));
MassProperties immovableMassProperties = new MassProperties(float.PositiveInfinity, Matrix.Identity);
plank.MassProperties = immovableMassProperties;
_physics.Add(plank);
var sphere = new SolidThing(this, sphereModel);
sphere.SetWorld(new Vector3(-4.9f, 0.0f, 9.0f), Quaternion.Identity);
_physics.Add(sphere);
// int size = 9;
//#if WINDOWS
//#else
// size = 4;
//#endif
// var models = new Model[] { cubeModel, sphereModel };
// for (int i = 0; i < size; i++)
// {
// for (int j = 0; j < size - i; j++)
// {
// for (int k = 0; k < size - i; k++)
// {
// var sphere = new SolidThing(this, i % 2 == 0 ? sphereModel : cubeModel);
// sphere.SetWorld(new Vector3(
// 0.501f * j + 0.25f * i,
// 0.501f * k + 0.25f * i,
// 0.501f * i + 0.5f
// ), Quaternion.Identity);
// _physics.Add(sphere);
// }
// }
// }
}
break;
case 6:
{
int size = 9;
#if WINDOWS
#else
size = 4;
#endif
var models = new Model[] { cubeModel, sphereModel, capsuleModel };
for (int i = 0; i < size; i++)
{
for (int j = 0; j < size - i; j++)
{
for (int k = 0; k < size - i; k++)
{
var sphere = new SolidThing(this, models[_rand.Next(3)]);
sphere.SetWorld(new Vector3(
0.501f * j + 0.25f * i,
0.501f * k + 0.25f * i,
1f * i + 0.5f));
_physics.Add(sphere);
}
}
}
}
break;
case 7:
{
var o = new SolidThing(this, torusModel);
o.SetWorld(new Vector3(0f, 0f, 0.5f), Quaternion.CreateFromAxisAngle(Vector3.UnitY, -MathHelper.PiOver2));
_physics.Add(o);
o = new SolidThing(this, torusModel);
o.SetWorld(new Vector3(0f, 0f, 4f), Quaternion.CreateFromAxisAngle(Vector3.UnitY, -MathHelper.PiOver4));
_physics.Add(o);
o = new SolidThing(this, slabModel);
o.SetWorld(new Vector3(-4f, 4f, 2f));
_physics.Add(o);
o = new SolidThing(this, this.Content.Load <Model>("models/cone"));
o.SetWorld(new Vector3(-4f, -4f, 1f), Quaternion.CreateFromAxisAngle(Vector3.UnitZ, MathHelper.PiOver2));
_physics.Add(o);
o = new SolidThing(this, cubeModel);
o.SetWorld(new Vector3(-4f, 6.1f, 3f));
_physics.Add(o);
}
break;
case 8:
{
RigidBody oLast = null;
for (int i = 0; i < 10; i++)
{
var o = new SolidThing(this, capsuleModel);
o.SetWorld(new Vector3(0f, 0f, 9.5f - i));
_physics.Add(o);
if (i == 0)
{
var j = new PointConstraint(o, room, new Vector3(0f, 0f, 10f));
j.IsCollisionEnabled = false;
_physics.Add(j);
}
else
{
var j = new PointConstraint(oLast, o, new Vector3(0f, 0f, 10f - (float)i));
j.IsCollisionEnabled = false;
_physics.Add(j);
}
oLast = o;
}
var a = new SolidThing(this, cubeModel);
a.SetWorld(new Vector3(1f, 0f, 0.25f));
_physics.Add(a);
var b = new SolidThing(this, cubeModel);
b.SetWorld(new Vector3(1f, 0f, 0.75f));
_physics.Add(b);
var j2 = new RevoluteJoint(b, a, new Vector3(1.25f, 0f, 0.5f), Vector3.UnitY,
0f, MathHelper.PiOver2);
j2.IsCollisionEnabled = false;
_physics.Add(j2);
a = new SolidThing(this, cubeModel);
a.SetWorld(new Vector3(1f, 1f, 0.25f));
_physics.Add(a);
b = new SolidThing(this, cubeModel);
b.SetWorld(new Vector3(1f, 1f, 0.75f));
_physics.Add(b);
var j4 = new GenericConstraint(b, a, new Frame(new Vector3(1f, 1f, 0.5f)),
Axes.All, Vector3.Zero, new Vector3(0f, 0f, 0.5f),
Axes.All, Vector3.Zero, Vector3.Zero);
j4.IsCollisionEnabled = false;
_physics.Add(j4);
a = new SolidThing(this, cubeModel);
a.SetWorld(new Vector3(1f, 2f, 0.25f));
_physics.Add(a);
b = new SolidThing(this, cubeModel);
b.SetWorld(new Vector3(1f, 2f, 0.75f));
_physics.Add(b);
var j5 = new GenericConstraint(b, a, new Frame(new Vector3(1f, 2f, 0.5f)),
Axes.All, new Vector3(-0.125f, -0.125f, 0f), new Vector3(0.125f, 0.125f, 0f),
Axes.All, Vector3.Zero, Vector3.Zero);
j5.IsCollisionEnabled = false;
_physics.Add(j5);
a = new SolidThing(this, sphereModel);
a.SetWorld(new Vector3(2f, 0f, 2f));
_physics.Add(a);
b = new SolidThing(this, sphereModel);
b.SetWorld(new Vector3(2f, 0f, 1f));
_physics.Add(b);
var g1 = new SpringForce(a, b, Vector3.Zero, Vector3.Zero, 1f, 5f, 0.05f);
_physics.Add(g1);
var j3 = new WorldPointConstraint(a, new Vector3(2f, 0f, 2f));
_physics.Add(j3);
}
break;
case 9:
{
var a = new SolidThing(this, sphereModel);
a.Skin.Remove(a.Skin[0]);
a.Skin.Add(new SpherePart(new Sphere(Vector3.Zero, 0.25f)), new Material(0f, 0.5f));
a.SetWorld(7.0f, new Vector3(0f, 0f, 5f), Quaternion.Identity);
a.MassProperties = MassProperties.Immovable;
_physics.Add(a);
_physics.Add(new SingularityForce(new Vector3(0f, 0f, 5f), 1E12f));
_physics.Gravity = Vector3.Zero;
var b = new SolidThing(this, cubeModel);
b.SetWorld(new Vector3(0f, 0f, 8f),
Quaternion.CreateFromAxisAngle(Vector3.UnitX, MathHelper.PiOver4 / 2.0f) *
Quaternion.CreateFromAxisAngle(Vector3.UnitY, MathHelper.PiOver4)
);
_physics.Add(b);
}
break;
default:
break;
}
}
public override ModelContent Process(NodeContent input, ContentProcessorContext context)
{
var attributes = input.Children.ToDictionary(n => n.Name, n => n.OpaqueData);
var nodesToRemove = (from node in input.Children
where node.OpaqueData.GetAttribute(TYPE_ATTR_NAME, MeshType.Both) == MeshType.Physical
select node).ToArray();
ModelContent model = base.Process(input, context);
var parts = new List <CompiledPart>();
var materials = new List <Material>();
var mass = new MassProperties();
var centerOfMass = Vector3.Zero;
foreach (var mesh in model.Meshes)
{
MeshType type = MeshType.Both;
PhysicalShape shape = PhysicalShape.Mesh;
float elasticity = _defaultElasticity, roughness = _defaultRoughness, density = _defaultDensity;
if (attributes.ContainsKey(mesh.Name))
{
type = attributes[mesh.Name].GetAttribute(TYPE_ATTR_NAME, MeshType.Both);
if (type == MeshType.Visual)
{
continue;
}
elasticity = attributes[mesh.Name].GetAttribute(ELASTICITY_ATTR_NAME, _defaultElasticity);
roughness = attributes[mesh.Name].GetAttribute(ROUGHNESS_ATTR_NAME, _defaultRoughness);
density = attributes[mesh.Name].GetAttribute(DENSITY_ATTR_NAME, _defaultDensity);
shape = attributes[mesh.Name].GetAttribute(SHAPE_ATTR_NAME, _defaultShape);
}
var meshCenterOfMass = Vector3.Zero;
var meshMass = MassProperties.Immovable;
CompiledPart meshPart = null;
if (mesh.MeshParts.Count < 1)
{
continue;
}
int[] indices = mesh.MeshParts[0].IndexBuffer.Skip(mesh.MeshParts[0].StartIndex).Take(mesh.MeshParts[0].PrimitiveCount * 3).ToArray();
Vector3[] vertices = MeshToVertexArray(context.TargetPlatform, mesh);
if (_windingOrder == WindingOrder.Clockwise)
{
ReverseWindingOrder(indices);
}
switch (shape)
{
case PhysicalShape.Mesh:
{
meshPart = new CompiledMesh(vertices, indices);
meshMass = MassProperties.Immovable;
meshCenterOfMass = GetMeshTranslation(mesh);
}
break;
case PhysicalShape.Polyhedron:
{
var hull = new ConvexHull3D(vertices);
meshPart = hull.ToPolyhedron();
meshMass = MassProperties.FromTriMesh(density, vertices, indices, out meshCenterOfMass);
}
break;
case PhysicalShape.Sphere:
{
Sphere s;
Sphere.Fit(vertices, out s);
meshPart = new CompiledSphere(s.Center, s.Radius);
meshMass = MassProperties.FromSphere(density, s.Center, s.Radius);
meshCenterOfMass = s.Center;
}
break;
case PhysicalShape.Capsule:
{
Capsule c;
Capsule.Fit(vertices, out c);
meshPart = new CompiledCapsule(c.P1, c.P2, c.Radius);
meshMass = MassProperties.FromCapsule(density, c.P1, c.P2, c.Radius, out meshCenterOfMass);
}
break;
}
parts.Add(meshPart);
materials.Add(new Material(elasticity, roughness));
Vector3.Multiply(ref meshCenterOfMass, meshMass.Mass, out meshCenterOfMass);
Vector3.Add(ref centerOfMass, ref meshCenterOfMass, out centerOfMass);
mass.Mass += meshMass.Mass;
meshMass.Inertia.M44 = 0f;
Matrix.Add(ref mass.Inertia, ref meshMass.Inertia, out mass.Inertia);
}
// compute mass properties
Vector3.Divide(ref centerOfMass, mass.Mass, out centerOfMass);
mass.Inertia.M44 = 1f;
MassProperties.TranslateInertiaTensor(ref mass.Inertia, -mass.Mass, centerOfMass, out mass.Inertia);
if (centerOfMass.Length() >= Constants.Epsilon)
{
var transform = Matrix.CreateTranslation(-centerOfMass.X, -centerOfMass.Y, -centerOfMass.Z);
foreach (var p in parts)
{
p.Transform(ref transform);
}
transform = model.Root.Transform;
transform.M41 -= centerOfMass.X;
transform.M42 -= centerOfMass.Y;
transform.M43 -= centerOfMass.Z;
model.Root.Transform = transform;
}
mass = new MassProperties(mass.Mass, mass.Inertia);
var rbm = new RigidBodyModel(mass, parts.ToArray(), materials.ToArray());
// remove non-visual nodes
if (nodesToRemove.Length > 0)
{
foreach (var node in nodesToRemove)
{
input.Children.Remove(node);
}
model = base.Process(input, context);
}
model.Tag = rbm;
return(model);
}
private JObject PhysicalPropertiesToJSON(MassProperties massProperties)
{
return(JObject.Parse(synFormatter.Format(ExportPhysicalProperties(massProperties))));
}
static void Main(string[] args)
{
// Initialize mass properties
MassProperties massProp = new MassProperties(1, 1);
Guid structureMassId = Guid.NewGuid();
massProp.SetMass(structureMassId, 4280); // kg, structure weight without fuel
SimpleGravity gravity = new SimpleGravity(new Vector2D(0, -1.62)); // m/s^2
// Initialize lander
Vector2D initalPosition = new Vector2D(0, 3000);
Vector2D initialVelocity = new Vector2D(0, -20);
double initialOrientation = Util.DegToRad(45);
RigidBody2D lander = new RigidBody2D(massProp, initalPosition, initialVelocity, initialOrientation, 0);
lander.Gravity = gravity;
// Initialize fuel tanks
FuelTank rcsTank = new FuelTank(massProp, 633);
FuelTank mainTank = new FuelTank(massProp, 10334);
// Initialize engines
Vector2D engineMountPoint = new Vector2D(0, -4); // m
double orientation = Util.DegToRad(-90);
double maxThrust = 45040; // N
double fuelUsage = 13; // kg/s, guess
Thruster mainEngine = new Thruster(massProp, lander, mainTank, engineMountPoint, orientation, maxThrust, fuelUsage, true);
// Thrusters
List <Vector2D> rcsMountPoints = new List <Vector2D>()
{
new Vector2D(-5, 0), new Vector2D(-5, 0), new Vector2D(5, 0), new Vector2D(5, 0)
};
List <double> rcsOrientations = new List <double>()
{
90, -90, 90, -90
};
List <Thruster> rcsThrusters = new List <Thruster>();
double rcsMaxThrust = 1760;
double rcsFuelUsage = 15; // kg/s
for (int i = 0; i < 4; i++)
{
rcsThrusters.Add(new Thruster(massProp, lander, rcsTank, rcsMountPoints[i], rcsOrientations[i], rcsMaxThrust, rcsFuelUsage, false));
}
// Flight control
LanderFlightControl flightControl = new LanderFlightControl(lander, mainEngine, rcsThrusters);
// logging
Logger log = new Logger(5);
log.MainEngine = mainEngine;
log.RigidBody = lander;
// Initialize scheduler
EqualScheduler scheduler = new EqualScheduler(40); //hz
scheduler.Add(flightControl, 0);
scheduler.Add(mainEngine, 10);
for (int i = 0; i < 4; i++)
{
scheduler.Add(rcsThrusters[i], 20 + i);
}
scheduler.Add(lander, 40);
scheduler.Add(log, 50);
// run sim
scheduler.Run(TimeSpan.FromSeconds(125));
log.WriteCSV("simdata_" + DateTime.Now.ToString("yyyyMMddTHHmmss") + ".csv");
}
