space-wizards/RobustToolbox
1
0
Fork 0
mirror of https://github.com/space-wizards/RobustToolbox.git synced 2026-02-15 03:30:53 +01:00
Code Issues Packages Projects Releases Wiki Activity

Fix centre of mass (#2212)

Browse Source
This commit is contained in:
metalgearsloth
2021-11-11 03:25:38 +11:00
committed by GitHub
parent c88498eca9
commit 4600f0531d
12 changed files with 251 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
Robust.Shared/GameObjects/Components/Collidable/PhysicsComponent.Physics.cs
View File

@@ -589,7 +589,7 @@ namespace Robust.Shared.GameObjects
[ViewVariables(VVAccess.ReadWrite)]
public float Inertia
{
get => _inertia + Mass * Vector2.Dot(LocalCenter, LocalCenter);
get => _inertia + _mass * Vector2.Dot(_localCenter, _localCenter);
set
{
DebugTools.Assert(!float.IsNaN(value));
@@ -600,7 +600,7 @@ namespace Robust.Shared.GameObjects
if (value > 0.0f && !_fixedRotation)
{
_inertia = value - Mass * Vector2.Dot(LocalCenter, LocalCenter);
_inertia = value - Mass * Vector2.Dot(_localCenter, _localCenter);
DebugTools.Assert(_inertia > 0.0f);
InvI = 1.0f / _inertia;
Dirty();
@@ -644,7 +644,6 @@ namespace Robust.Shared.GameObjects
[DataField("fixedRotation")]
private bool _fixedRotation = true;
// TODO: Will be used someday
/// <summary>
/// Get this body's center of mass offset to world position.
/// </summary>
@@ -658,6 +657,7 @@ namespace Robust.Shared.GameObjects
set
{
if (_bodyType != BodyType.Dynamic) return;
if (value.EqualsApprox(_localCenter)) return;
_localCenter = value;
@@ -991,7 +991,13 @@ namespace Robust.Shared.GameObjects
internal Transform GetTransform()
{
return new(Owner.Transform.WorldPosition, (float) Owner.Transform.WorldRotation.Theta);
var position = Owner.Transform.WorldPosition;
var angle = (float) Owner.Transform.WorldRotation.Theta;
var xf = new Transform(position, angle);
// xf.Position -= Transform.Mul(xf.Quaternion2D, LocalCenter);
return xf;
}
/// <summary>
@@ -1115,8 +1121,7 @@ namespace Robust.Shared.GameObjects
_invMass = 0.0f;
_inertia = 0.0f;
InvI = 0.0f;
LocalCenter = Vector2.Zero;
// Sweep
_localCenter = Vector2.Zero;
if (((int) _bodyType & (int) BodyType.Kinematic) != 0)
{
@@ -1124,33 +1129,18 @@ namespace Robust.Shared.GameObjects
}
var localCenter = Vector2.Zero;
var shapeManager = IoCManager.Resolve<IShapeManager>();
foreach (var fixture in _fixtures)
{
if (fixture.Mass <= 0.0f) continue;
var fixMass = fixture.Mass;
var data = new MassData {Mass = fixture.Mass};
shapeManager.GetMassData(fixture.Shape, ref data);
_mass += fixMass;
var center = Vector2.Zero;
// TODO: God this is garbage
switch (fixture.Shape)
{
case PhysShapeAabb aabb:
center = aabb.Centroid;
break;
case PolygonShape poly:
center = poly.Centroid;
break;
case PhysShapeCircle circle:
center = circle.Position;
break;
}
localCenter += center * fixMass;
_inertia += fixture.Inertia;
_mass += data.Mass;
localCenter += data.Center * data.Mass;
_inertia += data.I;
}
if (BodyType == BodyType.Static)
@@ -1173,7 +1163,7 @@ namespace Robust.Shared.GameObjects
if (_inertia > 0.0f && !_fixedRotation)
{
// Center inertia about center of mass.
_inertia -= _mass * Vector2.Dot(Vector2.Zero, Vector2.Zero);
_inertia -= _mass * Vector2.Dot(localCenter, localCenter);
DebugTools.Assert(_inertia > 0.0f);
InvI = 1.0f / _inertia;
@@ -1184,16 +1174,19 @@ namespace Robust.Shared.GameObjects
InvI = 0.0f;
}
LocalCenter = Vector2.Zero;
_localCenter = localCenter;
// TODO: Calculate Sweep
/*
var oldCenter = _sweep.Center;
_sweep.LocalCenter = localCenter;
_sweep.Center0 = _sweep.Center = Physics.Transform.Mul(GetTransform(), _sweep.LocalCenter);
var oldCenter = Sweep.Center;
Sweep.LocalCenter = localCenter;
Sweep.Center0 = Sweep.Center = Transform.Mul(GetTransform(), Sweep.LocalCenter);
*/
// Update center of mass velocity.
var a = _sweep.Center - oldCenter;
_linVelocity += new Vector2(-_angVelocity * a.y, _angVelocity * a.x);
*/
// _linVelocity += Vector2.Cross(_angVelocity, Worl - oldCenter);
}
/// <summary>

43
Robust.Shared/Physics/Collision/Shapes/PolygonShape.cs
View File

@@ -29,6 +29,7 @@ using Robust.Shared.Serialization;
using Robust.Shared.Serialization.Manager.Attributes;
using Robust.Shared.Utility;
using Robust.Shared.ViewVariables;
using Vector2 = Robust.Shared.Maths.Vector2;
namespace Robust.Shared.Physics.Collision.Shapes
{
@@ -127,53 +128,11 @@ namespace Robust.Shared.Physics.Collision.Shapes
Normals[i] = temp.Normalized;
}
Centroid = ComputeCentroid(Vertices);
// Compute the polygon mass data
// TODO: Update fixture. Maybe use events for it? Who tf knows.
// If we get grid polys then we'll actually need runtime updating of bbs.
}
private Vector2 ComputeCentroid(Vector2[] vertices)
{
var count = vertices.Length;
DebugTools.Assert(count >= 3);
var c = new Vector2(0.0f, 0.0f);
float area = 0.0f;
// Get a reference point for forming triangles.
// Use the first vertex to reduce round-off errors.
var s = vertices[0];
const float inv3 = 1.0f / 3.0f;
for (var i = 0; i < count; ++i)
{
// Triangle vertices.
var p1 = vertices[0] - s;
var p2 = vertices[i] - s;
var p3 = i + 1 < count ? vertices[i+1] - s : vertices[0] - s;
var e1 = p2 - p1;
var e2 = p3 - p1;
float D = Vector2.Cross(e1, e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
c += (p1 + p2 + p3) * triangleArea * inv3;
}
// Centroid
DebugTools.Assert(area > float.Epsilon);
c = c * (1.0f / area) + s;
return c;
}
public ShapeType ShapeType => ShapeType.Polygon;
public PolygonShape()

3
Robust.Shared/Physics/Dynamics/Contacts/ContactSolver.cs
View File

@@ -163,9 +163,6 @@ namespace Robust.Shared.Physics.Dynamics.Contacts
positionConstraint.IndexA = bodyA.IslandIndex[data.IslandIndex];
positionConstraint.IndexB = bodyB.IslandIndex[data.IslandIndex];
(positionConstraint.InvMassA, positionConstraint.InvMassB) = (invMassA, invMassB);
// TODO: Dis
// positionConstraint.LocalCenterA = bodyA._sweep.LocalCenter;
// positionConstraint.LocalCenterB = bodyB._sweep.LocalCenter;
positionConstraint.LocalCenterA = bodyA.LocalCenter;
positionConstraint.LocalCenterB = bodyB.LocalCenter;

4
Robust.Shared/Physics/Dynamics/Fixture.cs
View File

@@ -273,6 +273,7 @@ namespace Robust.Shared.Physics.Dynamics
fixture._hard = _hard;
fixture._collisionLayer = _collisionLayer;
fixture._collisionMask = _collisionMask;
fixture._mass = _mass;
}
// Moved from Shape because no MassData on Shape anymore (due to serv3 and physics ease-of-use etc etc.)
@@ -430,7 +431,8 @@ namespace Robust.Shared.Physics.Dynamics
_collisionMask == other.CollisionMask &&
Shape.Equals(other.Shape) &&
Body == other.Body &&
ID.Equals(other.ID);
ID.Equals(other.ID) &&
MathHelper.CloseTo(_mass, other._mass);
}
}

6
Robust.Shared/Physics/Dynamics/Joints/DistanceJoint.cs
View File

@@ -28,10 +28,8 @@ using System;
using Robust.Shared.Configuration;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Log;
using Robust.Shared.Maths;
using Robust.Shared.Serialization;
using Robust.Shared.Utility;
using Robust.Shared.ViewVariables;
namespace Robust.Shared.Physics.Dynamics.Joints
@@ -287,8 +285,8 @@ namespace Robust.Shared.Physics.Dynamics.Joints
{
_indexA = BodyA.IslandIndex[data.IslandIndex];
_indexB = BodyB.IslandIndex[data.IslandIndex];
_localCenterA = Vector2.Zero; //BodyA->m_sweep.localCenter;
_localCenterB = Vector2.Zero; //BodyB->m_sweep.localCenter;
_localCenterA = BodyA.LocalCenter;
_localCenterB = BodyB.LocalCenter;
_invMassA = BodyA.InvMass;
_invMassB = BodyB.InvMass;
_invIA = BodyA.InvI;

4
Robust.Shared/Physics/Dynamics/Joints/FrictionJoint.cs
View File

@@ -173,8 +173,8 @@ namespace Robust.Shared.Physics.Dynamics.Joints
{
_indexA = BodyA.IslandIndex[data.IslandIndex];
_indexB = BodyB.IslandIndex[data.IslandIndex];
_localCenterA = Vector2.Zero; // BodyA._sweep.LocalCenter;
_localCenterB = Vector2.Zero; //BodyB._sweep.LocalCenter;
_localCenterA = BodyA.LocalCenter;
_localCenterB = BodyB.LocalCenter;
_invMassA = BodyA.InvMass;
_invMassB = BodyB.InvMass;
_invIA = BodyA.InvI;

4
Robust.Shared/Physics/Dynamics/Joints/RevoluteJoint.cs
View File

@@ -141,8 +141,8 @@ namespace Robust.Shared.Physics.Dynamics.Joints
{
_indexA = BodyA.IslandIndex[data.IslandIndex];
_indexB = BodyB.IslandIndex[data.IslandIndex];
_localCenterA = Vector2.Zero; //BodyA->m_sweep.localCenter;
_localCenterB = Vector2.Zero; //BodyB->m_sweep.localCenter;
_localCenterA = BodyA.LocalCenter;
_localCenterB = BodyB.LocalCenter;
_invMassA = BodyA.InvMass;
_invMassB = BodyB.InvMass;
_invIA = BodyA.InvI;

38
Robust.Shared/Physics/Dynamics/MassData.cs Normal file
View File

@@ -0,0 +1,38 @@
// MIT License
// Copyright (c) 2019 Erin Catto
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
using Robust.Shared.Maths;
namespace Robust.Shared.Physics.Dynamics
{
public struct MassData
{
/// The mass of the shape, usually in kilograms.
public float Mass;
/// The position of the shape's centroid relative to the shape's origin.
public Vector2 Center;
/// The rotational inertia of the shape about the local origin.
public float I;
}
}

5
Robust.Shared/Physics/Dynamics/PhysicsIsland.cs
View File

@@ -337,7 +337,7 @@ stored in a single array since multiple arrays lead to multiple misses.
// Didn't use the old variable names because they're hard to read
var transform = _physicsManager.EnsureTransform(body);
var position = transform.Position;
var position = Transform.Mul(transform, body.LocalCenter);
// DebugTools.Assert(!float.IsNaN(position.X) && !float.IsNaN(position.Y));
var angle = transform.Quaternion2D.Angle;
@@ -516,6 +516,9 @@ stored in a single array since multiple arrays lead to multiple misses.
// Temporary NaN guards until PVS is fixed.
if (!float.IsNaN(bodyPos.X) && !float.IsNaN(bodyPos.Y))
{
var q = new Quaternion2D(angle);
bodyPos -= Transform.Mul(q, body.LocalCenter);
// body.Sweep.Center = bodyPos;
// body.Sweep.Angle = angle;

64
Robust.Shared/Physics/Dynamics/Sweep.cs
View File

@@ -20,26 +20,78 @@
* 3. This notice may not be removed or altered from any source distribution.
*/
using System;
using Robust.Shared.Maths;
using Robust.Shared.Utility;
namespace Robust.Shared.Physics.Dynamics
{
/// This describes the motion of a body/shape for TOI computation.
/// Shapes are defined with respect to the body origin, which may
/// no coincide with the center of mass. However, to support dynamics
/// we must interpolate the center of mass position.
internal struct Sweep
{
/// <summary>
/// Local center of mass position
/// </summary>
public Vector2 LocalCenter;
// AKA A in box2d
public float Angle;
// AKA A0 in box2d
/// <summary>
/// Fraction of the current time step in the range [0,1]
/// c0 and a0 are the positions at alpha0.
/// </summary>
public float Angle0;
public float Alpha0;
/// <summary>
/// Generally reflects the body's worldposition but not always.
/// Can be used to temporarily store it during CCD.
/// </summary>
// AKA C in box2d
public Vector2 Center;
public float Center0;
// AKA C0 in box2d
public Vector2 Center0;
// I Didn't copy LocalCenter because it's also on the Body and it will normally be rarely set sooooo
/// <summary>
/// Get the interpolated transform at a specific time.
/// </summary>
/// <param name="beta">beta is a factor in [0,1], where 0 indicates alpha0.</param>
/// <returns>the output transform</returns>
public Transform GetTransform(float beta)
{
var xf = new Transform(Center0 * (1.0f - beta) + Center * beta, (1.0f - beta) * Angle0 + beta * Angle);
// Shift to origin
xf.Position -= Transform.Mul(xf.Quaternion2D, LocalCenter);
return xf;
}
/// <summary>
/// Advance the sweep forward, yielding a new initial state.
/// </summary>
/// <param name="alpha">the new initial time.</param>
public void Advance(float alpha)
{
DebugTools.Assert(Alpha0 < 1.0f);
float beta = (alpha - Alpha0) / (1.0f - Alpha0);
Center0 += (Center - Center0) * beta;
Angle0 += beta * (Angle - Angle0);
Alpha0 = alpha;
}
/// <summary>
/// Normalize the angles.
/// </summary>
public void Normalize()
{
float twoPi = 2.0f * MathF.PI;
float d = twoPi * MathF.Floor(Angle0 / twoPi);
Angle0 -= d;
Angle -= d;
}
}
}

109
Robust.Shared/Physics/ShapeManager.cs
View File

@@ -1,6 +1,8 @@
using System;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Collision.Shapes;
using Robust.Shared.Physics.Dynamics;
using Robust.Shared.Utility;
namespace Robust.Shared.Physics
{
@@ -10,6 +12,8 @@ namespace Robust.Shared.Physics
/// Returns whether a particular point intersects the specified shape.
/// </summary>
bool TestPoint(IPhysShape shape, Transform xform, Vector2 worldPoint);
void GetMassData(IPhysShape shape, ref MassData data);
}
public class ShapeManager : IShapeManager
@@ -43,5 +47,110 @@ namespace Robust.Shared.Physics
throw new ArgumentOutOfRangeException($"No implemented TestPoint for {shape.GetType()}");
}
}
public void GetMassData(IPhysShape shape, ref MassData data)
{
DebugTools.Assert(data.Mass > 0f);
// Box2D just calls fixture.GetMassData which just calls the shape method anyway soooo
// we can just cut out the middle-man
switch (shape)
{
case EdgeShape edge:
data.Mass = 0.0f;
data.Center = (edge.Vertex1 + edge.Vertex2) * 0.5f;
data.I = 0.0f;
break;
case PhysShapeCircle circle:
// massData->mass = density * b2_pi * m_radius * m_radius;
data.Center = circle.Position;
// inertia about the local origin
data.I = data.Mass * (0.5f * circle.Radius * circle.Radius + Vector2.Dot(circle.Position, circle.Position));
break;
case PhysShapeAabb aabb:
var polygon = (PolygonShape) aabb;
GetMassData(polygon, ref data);
break;
case PolygonShape poly:
// Polygon mass, centroid, and inertia.
// Let rho be the polygon density in mass per unit area.
// Then:
// mass = rho * int(dA)
// centroid.x = (1/mass) * rho * int(x * dA)
// centroid.y = (1/mass) * rho * int(y * dA)
// I = rho * int((x*x + y*y) * dA)
//
// We can compute these integrals by summing all the integrals
// for each triangle of the polygon. To evaluate the integral
// for a single triangle, we make a change of variables to
// the (u,v) coordinates of the triangle:
// x = x0 + e1x * u + e2x * v
// y = y0 + e1y * u + e2y * v
// where 0 <= u && 0 <= v && u + v <= 1.
//
// We integrate u from [0,1-v] and then v from [0,1].
// We also need to use the Jacobian of the transformation:
// D = cross(e1, e2)
//
// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
//
// The rest of the derivation is handled by computer algebra.
var count = poly.Vertices.Length;
DebugTools.Assert(count >= 3);
Vector2 center = new(0.0f, 0.0f);
float area = 0.0f;
float I = 0.0f;
// Get a reference point for forming triangles.
// Use the first vertex to reduce round-off errors.
var s = poly.Vertices[0];
const float k_inv3 = 1.0f / 3.0f;
for (var i = 0; i < count; ++i)
{
// Triangle vertices.
var e1 = poly.Vertices[i] - s;
var e2 = i + 1 < count ? poly.Vertices[i+1] - s : poly.Vertices[0] - s;
float D = Vector2.Cross(e1, e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += (e1 + e2) * triangleArea * k_inv3;
float ex1 = e1.X, ey1 = e1.Y;
float ex2 = e2.X, ey2 = e2.Y;
float intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;
float inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;
I += (0.25f * k_inv3 * D) * (intx2 + inty2);
}
// Total mass
// data.Mass = density * area;
var density = data.Mass / area;
// Center of mass
DebugTools.Assert(area > float.Epsilon);
center *= 1.0f / area;
data.Center = center + s;
// Inertia tensor relative to the local origin (point s).
data.I = density * I;
// Shift to center of mass then to original body origin.
data.I += data.Mass * (Vector2.Dot(data.Center, data.Center) - Vector2.Dot(center, center));
break;
default:
throw new NotImplementedException($"Cannot get MassData for {shape} as it's not implemented!");
}
}
}
}

4
Robust.UnitTesting/Shared/GameObjects/Systems/AnchoredSystemTests.cs
View File

@@ -35,6 +35,10 @@ namespace Robust.UnitTesting.Shared.GameObjects.Systems
{
f.LoadString(Prototypes);
})
.RegisterDependencies(f =>
{
f.Register<IShapeManager, ShapeManager>();
})
.InitializeInstance();
var mapManager = sim.Resolve<IMapManager>();