public void Deconstruct(out ShadowCornerManifold m, out EdgeMount mount1, out EdgeMount mount2, out EdgeMount?mount3, out ThreeWayPenConstraint.Partial partialConstraint)
{
m = this.m;
mount1 = this.mount1;
mount2 = this.mount2;
mount3 = this.mount3;
partialConstraint = this.partialConstraint;
}
public void DebugCollect(ShadowCornerManifold manifold, EdgeMount mount1, EdgeMount mount2, EdgeMount?mount3, ThreeWayPenConstraint.Partial partial)
{
if (debugCornerMounts != null)
{
debugCornerMounts.Add(new CornerMountTuple {
m = manifold, mount1 = mount1, mount2 = mount2, mount3 = mount3, partialConstraint = partial
});
}
}
void OnDrawGizmos()
{
if (Application.isPlaying && enable)
{
Gizmos.color = color;
var world = World.DefaultGameObjectInjectionWorld;
var massGetter = world.GetOrCreateSystem <MassGetter>();
var masses = massGetter.GetComponentDataFromEntity <Mass>();
var velocities = new Dictionary <Entity, Velocity>();
foreach (var tup in world.GetOrCreateSystem <ShadowConstraintSystem>().GetCornerMountsForDebug())
{
var(manifold, mount1, mount2, mount3, pConstraint) = tup;
// Recalculating pConstraint to have bias such that a complete resolution occurs
var prototype = new ThreeWayPenConstraint.Partial.Prototype(in manifold);
if (mount3 != null)
{
Debug.Log("JacobianGizmoDrawer does not support triple shadow edge constraints");
//pConstraint = new ShadowCornerConstraint.Partial(prototype, mount1, mount2, mount3.Value, float2.zero, manifold);
}
else
{
pConstraint = new ThreeWayPenConstraint.Partial(prototype, mount1, mount2, pConstraint.e3, manifold);
}
var constraint = new ThreeWayPenConstraint(pConstraint, masses, dt: 1, beta: 1, delta_slop: 0);
Velocity GetVelocity(Entity e)
{
if (velocities.TryGetValue(e, out var vel))
{
return(vel);
}
else
{
return(new Velocity());
}
}
void SetVelocity(Entity e, Velocity v)
{
velocities[e] = v;
}
velocities.Clear();
Velocity v1;
Velocity v2;
Velocity v3;
for (int i = 0; i < numIterations; i++)
{
v1 = GetVelocity(constraint.e1);
v2 = GetVelocity(constraint.e2);
v3 = GetVelocity(constraint.e3);
constraint.ApplyImpulses(ref v1, ref v2, ref v3, dt: 1);
velocities.Clear();
SetVelocity(constraint.e1, v1);
SetVelocity(constraint.e2, v2);
SetVelocity(constraint.e3, v3);
}
v1 = GetVelocity(constraint.e1);
v2 = GetVelocity(constraint.e2);
v3 = GetVelocity(constraint.e3);
DrawWireframeShadowEdgeGizmo(world, constraint.e1,
source: manifold.x1,
edgeDir: manifold.d1,
mount: mount1.point,
posOffset: v1.vel,
rotOffset: v1.angVel
);
DrawWireframeShadowEdgeGizmo(world, constraint.e2,
source: manifold.x2,
edgeDir: manifold.d2,
mount: mount2.point,
posOffset: v2.vel,
rotOffset: v2.angVel
);
DrawWireframeBoxGizmo(world, constraint.e3, v3.vel, v3.angVel);
}
}
}
