public static HydroTimeSeries GetMissingValuesHydro(ITimeSeries ts, DateTime start, DateTime end, TimeStep step)
{
HydroTimeSeries missingTs = new HydroTimeSeries(start, end);
List<int> breakIx = GetDataBreaks(ts);
//TODO missing points before series start
foreach (int begIndex in breakIx)
{
if (begIndex < ts.Count - 1)
{
DateTime begDate = DateTime.FromOADate(ts[begIndex].X);
DateTime endDate = DateTime.FromOADate(ts[begIndex + 1].X);
if (step == TimeStep.Day)
{
int nd = (int)endDate.Subtract(begDate).TotalDays;
for (int i = 0; i < nd; i++)
{
DateTime newTime = begDate.AddDays(i);
missingTs.AddUnknownValue(newTime);
}
}
else
{
int nh = (int)endDate.Subtract(begDate).TotalHours;
for (int i = 0; i < nh; i++)
{
DateTime newTime = begDate.AddHours(i);
missingTs.AddUnknownValue(newTime);
}
}
}
}
return missingTs;
}
/// <summary>
/// Creates a new regular-spaced list and sets all values to initialValue
/// </summary>
/// <param name="period"></param>
/// <param name="step"></param>
/// <param name="initialValue"></param>
/// <returns></returns>
public TimeValueList(TimeInterval period, TimeStep step, double initialValue)
{
_timeStepFactor = (step == TimeStep.Hour) ? 24 : 1;
_start = period.Start.ToOADate();
switch(TimeStep)
{
case TimeStep.Hour:
_data = new double[(int) period.Length.TotalHours];
break;
case TimeStep.Day:
_data = new double[(int) period.Length.TotalDays];
break;
default:
throw new ArgumentException("'step' parameter must be 'Hour' or 'Day'");
}
if (initialValue != 0.0)
{
// initialize array values...
for (int i = 0; i < _data.Length; ++i)
{
_data[i] = initialValue;
}
}
}
/// <summary>
/// Fills the data gaps inside the time-series by setting them to 'no data' vals
/// </summary>
/// <param name="ts"></param>
public static ITimeSeries FillDataGaps(ITimeSeries ts, TimeStep step)
{
ITimeSeries myTs = MakeRegularTimeSeries(ts.Start, ts.End, step);
if (step == TimeStep.Day)
{
double tStart = myTs[0].X;
for (int i = 0; i < ts.Count - 1; i++)
{
double index = (ts[i].X - tStart);
int ix = (int)index;
myTs[ix].Y = ts[i].Y;
}
}
else
{
double tStart = myTs[0].X * 24;
for (int i = 0; i < ts.Count - 1; i++)
{
double index = (ts[i].X * 24 - tStart);
int ix = (int)index;
myTs[ix].Y = ts[i].Y;
}
}
return myTs;
}
public ServiceRate(string serviceName,TimeStep timeStep,DateTime stamp,int rate)
{
this.ServiceName = serviceName;
this.RateTimeStep = timeStep;
this.TimeStamp = stamp;
this.Rate = rate;
}
public override void Step(TimeStep step)
{
if (_bodyList == null) return;
if (useWorldGravity)
{
gravity = _world.Gravity;
}
for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
{
Body body = i.body;
if (body.IsSleeping())
{
//Buoyancy force is just a function of position,
//so unlike most forces, it is safe to ignore sleeping bodes
continue;
}
Vec2 areac = new Vec2(0, 0);
Vec2 massc = new Vec2(0, 0);
float area = 0;
float mass = 0;
for (Shape shape = body.GetShapeList(); shape != null; shape = shape.GetNext())
{
Vec2 sc;
float sarea = shape.ComputeSubmergedArea(normal, offset, body.GetXForm(), out sc);
area += sarea;
areac.X += sarea * sc.X;
areac.Y += sarea * sc.Y;
float shapeDensity = 0;
if (useDensity)
{
//TODO: Expose density publicly
shapeDensity = shape.Density;
}
else
{
shapeDensity = 1;
}
mass += sarea * shapeDensity;
massc.X += sarea * sc.X * shapeDensity;
massc.Y += sarea * sc.Y * shapeDensity;
}
areac.X /= area;
areac.Y /= area;
massc.X /= mass;
massc.Y /= mass;
if (area < Box2DX.Common.Settings.FLT_EPSILON)
continue;
//Buoyancy
Vec2 buoyancyForce = -density * area * gravity;
body.ApplyForce(buoyancyForce, massc);
//Linear drag
Vec2 dragForce = body.GetLinearVelocityFromWorldPoint(areac) - velocity;
dragForce *= -linearDrag * area;
body.ApplyForce(dragForce, areac);
//Angular drag
//TODO: Something that makes more physical sense?
body.ApplyTorque(-body.GetInertia() / body.GetMass() * area * body.GetAngularVelocity() * angularDrag);
}
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
_jointError = BodyA.Sweep.A - TargetAngle;
_bias = -BiasFactor * step.inv_dt * _jointError;
_massFactor = (1 - Softness) / (BodyA.InvI);
}
public override void Step(TimeStep step)
{
for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
{
Body body = i.body;
if (body.IsSleeping())
continue;
body.SetLinearVelocity(body.GetLinearVelocity() + step.Dt * A);
}
}
public override void Step(TimeStep step)
{
//B2_NOT_USED(step);
for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
{
Body body = i.body;
if (body.IsSleeping())
continue;
body.ApplyForce(F, body.GetWorldCenter());
}
}
public static void ReadBinaryFile(string fileName, DateTime startTime, DateTime endTime, TimeStep timeStep,
bool includeNA, IObservationList observations)
{
if (timeStep == TimeStep.Day)
{
ReadBinaryFileDaily(fileName, startTime, endTime, includeNA, observations);
}
else
{
ReadBinaryFileHourly(fileName, startTime, endTime, includeNA, observations);
}
}
protected internal override void RunLogic(TimeStep step)
{
foreach (Body e in this.Bodies)
{
if (e.IgnoresGravity ||
e.IgnoresPhysicsLogics)
{
continue;
}
Vector2D.Add(ref e.State.Acceleration.Linear, ref gravity, out e.State.Acceleration.Linear);
}
}
protected internal override void RunLogic(TimeStep step)
{
foreach (Body e in this.Bodies)
{
if (e.IgnoresGravity ||
e.IgnoresPhysicsLogics)
{
continue;
}
Vector2D vect;
Vector2D.Subtract(ref location, ref e.State.Position.Linear, out vect);
Vector2D.Normalize(ref vect, out vect);
Vector2D.Multiply(ref vect, ref gravity, out vect);
Vector2D.Add(ref e.State.Acceleration.Linear, ref vect, out e.State.Acceleration.Linear);
}
}
public override void Step(TimeStep step)
{
float timestep = step.Dt;
if (timestep <= Settings.FLT_EPSILON)
return;
if (timestep > MaxTimestep && MaxTimestep > 0)
timestep = MaxTimestep;
for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
{
Body body = i.body;
if (body.IsSleeping())
continue;
Vec2 damping = body.GetWorldVector(Math.Mul(T, body.GetLocalVector(body.GetLinearVelocity())));
body.SetLinearVelocity(body.GetLinearVelocity() + timestep*damping);
}
}
public override void Detect(TimeStep step)
{
for (int index1 = 0; index1 < this.Bodies.Count; index1++)
{
Body body1 = this.Bodies[index1];
for (int index2 = index1 + 1; index2 < this.Bodies.Count; index2++)
{
Body body2 = this.Bodies[index2];
if ((body1.Mass.MassInv != 0 || body2.Mass.MassInv != 0) &&
Body.CanCollide(body1, body2) &&
body1.Rectangle.Intersects(body2.Rectangle))
{
OnCollision(step, body1, body2);
}
}
}
}
protected internal override void RunLogic(TimeStep step)
{
foreach (Body e in Bodies)
{
if (e == body ||
e.IgnoresGravity ||
e.IgnoresPhysicsLogics)
{
continue;
}
Scalar magnitude;
Vector2D gravity;
Vector2D.Subtract(ref body.State.Position.Linear, ref e.State.Position.Linear, out gravity);
Vector2D.Normalize(ref gravity, out magnitude, out gravity);
magnitude = (body.Mass.AccelerationDueToGravity /
(magnitude * magnitude * metersPerDistanceUnit * metersPerDistanceUnit));
Vector2D.Multiply(ref gravity, ref magnitude, out gravity);
Vector2D.Add(ref e.State.Acceleration.Linear, ref gravity, out e.State.Acceleration.Linear);
}
}
/// <summary>
/// Retrieves a time series of discharge observations from the database
/// (only measured, non-zero values are retrieved)
/// </summary>
/// <param name="stationId"></param>
/// <param name="variableId"></param>
/// <param name="start"></param>
/// <param name="end"></param>
/// <param name="scaleFactor"></param>
/// <param name="observations"></param>
public static void LoadObservationsDischarge(int stationId, int variableId, DateTime start, DateTime end,
TimeStep step, IObservationList observations)
{
//observations.Clear();
SqlCommand cmd = DataUtils.CreateCommand();
cmd.CommandText = "plaveninycz.new_query_observations";
cmd.CommandType = CommandType.StoredProcedure;
SetCmdParameters(cmd, stationId, variableId, start, end, step);
SqlDataReader rdr;
double val;
DateTime t;
try
{
cmd.Connection.Open();
rdr = cmd.ExecuteReader(CommandBehavior.CloseConnection);
while (rdr.Read())
{
if (!rdr.IsDBNull(1))
{
t = Convert.ToDateTime(rdr[0]);
val = Convert.ToDouble(rdr[1]);
//val = Math.Pow(2.0, (val / 1000.0));
if (val > 0)
{
observations.AddObservation(t, val);
}
else
{
observations.AddUnknownValue(t);
}
}
}
rdr.Close();
}
finally
{
cmd.Connection.Close();
}
}
public override void Step(TimeStep step)
{
//B2_NOT_USED(step);
if (InvSqr)
{
for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
{
Body body1 = i.body;
for (ControllerEdge j = _bodyList; j != i; j = j.nextBody)
{
Body body2 = j.body;
Vector2 d = body2.GetWorldCenter() - body1.GetWorldCenter();
float r2 = d.sqrMagnitude;
if (r2 < Settings.FLT_EPSILON)
continue;
Vector2 f = G / r2 / Math.Sqrt(r2) * body1.GetMass() * body2.GetMass() * d;
body1.ApplyForce(f, body1.GetWorldCenter());
body2.ApplyForce(-1.0f * f, body2.GetWorldCenter());
}
}
}
else
{
for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
{
Body body1 = i.body;
for (ControllerEdge j = _bodyList; j != i; j = j.nextBody)
{
Body body2 = j.body;
Vector2 d = body2.GetWorldCenter() - body1.GetWorldCenter();
float r2 = d.sqrMagnitude;
if (r2 < Settings.FLT_EPSILON)
continue;
Vector2 f = G / r2 * body1.GetMass() * body2.GetMass() * d;
body1.ApplyForce(f, body1.GetWorldCenter());
body2.ApplyForce(-1.0f * f, body2.GetWorldCenter());
}
}
}
}
protected internal override void RunLogic(TimeStep step)
{
foreach (Body e in Bodies)
{
if (e.IgnoresPhysicsLogics) { continue; }
Scalar velocity;
Vector2D.GetMagnitude(ref e.State.Velocity.Linear, out velocity);
if (velocity > maxLinearVelocity)
{
velocity = maxLinearVelocity / velocity;
Vector2D.Multiply(ref e.State.Velocity.Linear, ref velocity, out e.State.Velocity.Linear);
}
if (e.State.Velocity.Angular > maxAngularVelocity)
{
e.State.Velocity.Angular = maxAngularVelocity;
}
else if (e.State.Velocity.Angular < -maxAngularVelocity)
{
e.State.Velocity.Angular = -maxAngularVelocity;
}
}
}
private void Run([NotNull] CalcLoadTypeDto dstLoadType,
[NotNull][ItemNotNull] List <OnlineEnergyFileRow> energyFileRows,
[NotNull] IFileFactoryAndTracker fft,
[NotNull] EnergyFileColumns efc,
[NotNull] Dictionary <CalcLoadTypeDto, Dictionary <StrGuid, double> > loadTypeTodeviceIDToAverageLookup,
[ItemNotNull][NotNull] List <DeviceTaggingSetInformation> deviceTaggingSets,
[NotNull] Dictionary <string, string> deviceNameToCategory,
[NotNull] Dictionary <string, double> deviceEnergyDict,
[NotNull] HouseholdKey key)
{
if (!efc.ColumnEntriesByColumn.ContainsKey(dstLoadType))
{
//for this load type for this house there are no column, so nothing to do
return;
}
var calcParameters = Repository.CalcParameters;
var rowlength = energyFileRows[0].EnergyEntries.Count;
var ts = new TimeStep(0, 0, true);
var sum = new OnlineEnergyFileRow(ts, new List <double>(new double[rowlength]), dstLoadType);
var curDate = calcParameters.OfficialStartTime;
var sumsPerMonth = MakeSumsPerMonth(dstLoadType, energyFileRows, curDate, sum, rowlength);
/*
* if (Config.IsInUnitTesting && Config.ExtraUnitTestChecking) {
* if (!double.IsNaN(previousTotal) && Math.Abs(sum.SumFresh - previousTotal) > 0.000001) {
* throw new LPGException("Unknown bug while generating the device totals. Sums don't match.");
* }
* }*/
var sumPerMonthPerDeviceID = MakeSumPerMonthPerDeviceID(dstLoadType, efc, sumsPerMonth, out var columns);
var sumsPerDeviceID = new Dictionary <StrGuid, double>();
var deviceNamesPerID = new Dictionary <StrGuid, string>();
var sumPerDeviceName = new Dictionary <string, double>();
foreach (var pair in columns)
{
var ce = pair.Value;
if (!sumsPerDeviceID.ContainsKey(ce.DeviceGuid))
{
sumsPerDeviceID.Add(ce.DeviceGuid, 0);
deviceNamesPerID.Add(ce.DeviceGuid, ce.Name);
}
if (!sumPerDeviceName.ContainsKey(ce.Name))
{
sumPerDeviceName.Add(ce.Name, 0);
}
sumPerDeviceName[ce.Name] += sum.EnergyEntries[pair.Key];
sumsPerDeviceID[ce.DeviceGuid] += sum.EnergyEntries[pair.Key];
}
MakeTotalsPerDeviceTaggingSet(fft, dstLoadType, deviceTaggingSets, deviceEnergyDict, key);
var builder = new StringBuilder();
foreach (var calcDeviceTaggingSet in deviceTaggingSets)
{
if (calcDeviceTaggingSet.LoadTypesForThisSet.Any(x => x.Name == dstLoadType.Name))
{
builder.Append(calcDeviceTaggingSet.Name).Append(calcParameters.CSVCharacter);
}
}
var taggingsetHeader = builder.ToString();
var devicesums = fft.MakeFile <StreamWriter>("DeviceSums." + dstLoadType.Name + "." + key.Key + ".csv",
"Summed up " + dstLoadType.Name + " use per device and comparison with statistical values",
true,
ResultFileID.DeviceSums,
key,
TargetDirectory.Reports,
calcParameters.InternalStepsize, CalcOption.TotalsPerDevice,
dstLoadType.ConvertToLoadTypeInformation());
var calcDuration = calcParameters.OfficialEndTime - calcParameters.OfficialStartTime;
var amountofYears = calcDuration.TotalDays / 365.0;
var sb = new StringBuilder();
sb.Append("Device name");
sb.Append(calcParameters.CSVCharacter);
sb.Append("Usage sum in this simulation [").Append(dstLoadType.UnitOfSum).Append("]");
sb.Append(calcParameters.CSVCharacter);
sb.Append("Usage sum in this simulation linear extrapolated to 1 year [").Append(dstLoadType.UnitOfSum).Append("]");
sb.Append(calcParameters.CSVCharacter);
sb.Append("Comparison Value from the device entry [").Append(dstLoadType.UnitOfSum).Append("]");
sb.Append(calcParameters.CSVCharacter);
sb.Append("Percentage of the comparison value [1 = 100%]");
sb.Append(calcParameters.CSVCharacter);
sb.Append("Device Category");
sb.Append(calcParameters.CSVCharacter);
sb.Append(taggingsetHeader);
devicesums.WriteLine(sb);
double devicesum = 0;
double extrapolatedSum = 0;
double comparsionvaluessum = 0;
foreach (var keyValuePair in sumsPerDeviceID)
{
var s = string.Empty;
s += deviceNamesPerID[keyValuePair.Key];
s += calcParameters.CSVCharacter;
s += keyValuePair.Value * dstLoadType.ConversionFactor;
devicesum += keyValuePair.Value;
//deviceSums.AddDeviceSum(deviceNamesPerID[keyValuePair.Key],devicesum,dstLoadType);
s += calcParameters.CSVCharacter;
var extrapolatedValue = keyValuePair.Value * dstLoadType.ConversionFactor / amountofYears;
s += extrapolatedValue;
extrapolatedSum += keyValuePair.Value / amountofYears;
s += calcParameters.CSVCharacter;
double defaultvalue = 0;
if (loadTypeTodeviceIDToAverageLookup.ContainsKey(dstLoadType))
{
if (loadTypeTodeviceIDToAverageLookup[dstLoadType].ContainsKey(keyValuePair.Key))
{
defaultvalue = loadTypeTodeviceIDToAverageLookup[dstLoadType][keyValuePair.Key];
}
}
s += defaultvalue;
comparsionvaluessum += defaultvalue;
s += calcParameters.CSVCharacter;
if (Math.Abs(defaultvalue) > Constants.Ebsilon)
{
s += extrapolatedValue / defaultvalue;
}
else
{
s += 0;
}
s += calcParameters.CSVCharacter;
var devicename = deviceNamesPerID[keyValuePair.Key];
var deviceCategory = "(no category)";
if (deviceNameToCategory.ContainsKey(devicename))
{
deviceCategory = deviceNameToCategory[devicename];
}
s += deviceCategory;
s += calcParameters.CSVCharacter;
var tags = string.Empty;
foreach (var calcDeviceTaggingSet in deviceTaggingSets)
{
if (calcDeviceTaggingSet.LoadTypesForThisSet.Any(x => x.Name == dstLoadType.Name))
{
var deviceName = deviceNamesPerID[keyValuePair.Key];
if (calcDeviceTaggingSet.TagByDeviceName.ContainsKey(deviceName))
{
tags += calcDeviceTaggingSet.TagByDeviceName[deviceName] + calcParameters.CSVCharacter;
}
else
{
tags += Constants.UnknownTag + calcParameters.CSVCharacter;
}
}
}
devicesums.WriteLine(s + tags);
}
var sumstr = "Sums";
sumstr += calcParameters.CSVCharacter;
sumstr += devicesum * dstLoadType.ConversionFactor;
sumstr += calcParameters.CSVCharacter;
sumstr += extrapolatedSum * dstLoadType.ConversionFactor;
sumstr += calcParameters.CSVCharacter;
sumstr += comparsionvaluessum;
devicesums.WriteLine(sumstr);
devicesums.Flush();
WriteMonthlyDeviceSums(fft, dstLoadType, sumPerMonthPerDeviceID, deviceNamesPerID, key);
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
float K = 0.0f;
_J.SetZero();
if (_revolute1 != null || _fixedRevolute1 != null)
{
_J.AngularA = -1.0f;
K += b1.InvI;
}
else
{
Vector2 ug;
if (_prismatic1 != null)
{
ug = _prismatic1.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
}
else
{
ug = _fixedPrismatic1.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
}
Transform xf1 /*, xfg1*/;
b1.GetTransform(out xf1);
//g1.GetTransform(out xfg1);
Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchor1 - b1.LocalCenter);
float crug = MathUtils.Cross(r, ug);
_J.LinearA = -ug;
_J.AngularA = -crug;
K += b1.InvMass + b1.InvI * crug * crug;
}
if (_revolute2 != null || _fixedRevolute2 != null)
{
_J.AngularB = -Ratio;
K += Ratio * Ratio * b2.InvI;
}
else
{
Vector2 ug;
if (_prismatic2 != null)
{
ug = _prismatic2.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
}
else
{
ug = _fixedPrismatic2.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1);
}
Transform /*xfg1,*/ xf2;
//g1.GetTransform(out xfg1);
b2.GetTransform(out xf2);
Vector2 r = MathUtils.Multiply(ref xf2.R, LocalAnchor2 - b2.LocalCenter);
float crug = MathUtils.Cross(r, ug);
_J.LinearB = -Ratio * ug;
_J.AngularB = -Ratio * crug;
K += Ratio * Ratio * (b2.InvMass + b2.InvI * crug * crug);
}
// Compute effective mass.
Debug.Assert(K > 0.0f);
_mass = K > 0.0f ? 1.0f / K : 0.0f;
if (Settings.EnableWarmstarting)
{
// Warm starting.
b1.LinearVelocityInternal += b1.InvMass * _impulse * _J.LinearA;
b1.AngularVelocityInternal += b1.InvI * _impulse * _J.AngularA;
b2.LinearVelocityInternal += b2.InvMass * _impulse * _J.LinearB;
b2.AngularVelocityInternal += b2.InvI * _impulse * _J.AngularB;
}
else
{
_impulse = 0.0f;
}
}
internal void Reset(ref TimeStep step, int count, Contact[] contacts, SolverPosition[] positions, SolverVelocity[] velocities,
int[] locks, int velocityConstraintsMultithreadThreshold, int positionConstraintsMultithreadThreshold)
{
_count = count;
_positions = positions;
_velocities = velocities;
_locks = locks;
_contacts = contacts;
_velocityConstraintsMultithreadThreshold = velocityConstraintsMultithreadThreshold;
_positionConstraintsMultithreadThreshold = positionConstraintsMultithreadThreshold;
// grow the array
if (_velocityConstraints == null || _velocityConstraints.Length < count)
{
int newBufferCount = Math.Max(count, 32);
newBufferCount = newBufferCount + (newBufferCount * 2 >> 4); // grow by x1.125f
newBufferCount = (newBufferCount + 31) & (~31); // grow in chunks of 32.
int oldBufferCount = (_velocityConstraints == null) ? 0 : _velocityConstraints.Length;
Array.Resize(ref _velocityConstraints, newBufferCount);
Array.Resize(ref _positionConstraints, newBufferCount);
for (int i = oldBufferCount; i < newBufferCount; i++)
{
_velocityConstraints[i] = new ContactVelocityConstraint();
_positionConstraints[i] = new ContactPositionConstraint();
}
}
// Initialize position independent portions of the constraints.
for (int i = 0; i < _count; ++i)
{
Contact contact = contacts[i];
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
Shape shapeA = fixtureA.Shape;
Shape shapeB = fixtureB.Shape;
float radiusA = shapeA.Radius;
float radiusB = shapeB.Radius;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
Manifold manifold = contact.Manifold;
int pointCount = manifold.PointCount;
Debug.Assert(pointCount > 0);
ContactVelocityConstraint vc = _velocityConstraints[i];
vc.friction = contact.Friction;
vc.restitution = contact.Restitution;
vc.tangentSpeed = contact.TangentSpeed;
vc.indexA = bodyA.IslandIndex;
vc.indexB = bodyB.IslandIndex;
vc.invMassA = bodyA._invMass;
vc.invMassB = bodyB._invMass;
vc.invIA = bodyA._invI;
vc.invIB = bodyB._invI;
vc.contactIndex = i;
vc.pointCount = pointCount;
vc.K.SetZero();
vc.normalMass.SetZero();
ContactPositionConstraint pc = _positionConstraints[i];
pc.indexA = bodyA.IslandIndex;
pc.indexB = bodyB.IslandIndex;
pc.invMassA = bodyA._invMass;
pc.invMassB = bodyB._invMass;
pc.localCenterA = bodyA._sweep.LocalCenter;
pc.localCenterB = bodyB._sweep.LocalCenter;
pc.invIA = bodyA._invI;
pc.invIB = bodyB._invI;
pc.localNormal = manifold.LocalNormal;
pc.localPoint = manifold.LocalPoint;
pc.pointCount = pointCount;
pc.radiusA = radiusA;
pc.radiusB = radiusB;
pc.type = manifold.Type;
for (int j = 0; j < pointCount; ++j)
{
ManifoldPoint cp = manifold.Points[j];
VelocityConstraintPoint vcp = vc.points[j];
if (step.warmStarting)
{
vcp.normalImpulse = step.dtRatio * cp.NormalImpulse;
vcp.tangentImpulse = step.dtRatio * cp.TangentImpulse;
}
else
{
vcp.normalImpulse = 0.0f;
vcp.tangentImpulse = 0.0f;
}
vcp.rA = Vector2.Zero;
vcp.rB = Vector2.Zero;
vcp.normalMass = 0.0f;
vcp.tangentMass = 0.0f;
vcp.velocityBias = 0.0f;
pc.localPoints[j] = cp.LocalPoint;
}
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b = BodyA;
Transform xf1;
b.GetTransform(out xf1);
Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b.LocalCenter);
// Cdot = v + cross(w, r)
Vector2 Cdot = b.LinearVelocityInternal + MathUtils.Cross(b.AngularVelocityInternal, r);
Vector2 impulse = MathUtils.Multiply(ref _mass, -(Cdot + _beta*_C + _gamma*_impulse));
Vector2 oldImpulse = _impulse;
_impulse += impulse;
float maxImpulse = step.dt*MaxForce;
if (_impulse.LengthSquared() > maxImpulse*maxImpulse)
{
_impulse *= maxImpulse/_impulse.Length();
}
impulse = _impulse - oldImpulse;
b.LinearVelocityInternal += b.InvMass*impulse;
b.AngularVelocityInternal += b.InvI*MathUtils.Cross(r, impulse);
}
private bool PassedFirstRepairTime(DateTime start, TimeStep step) => step.Prev - start < FirstRepairTime && step.Now - start >= FirstRepairTime;
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
if (_enableMotor || _enableLimit)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
Box2DNetDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
}
// Compute the effective mass matrix.
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2]
// [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2]
// [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2]
float m1 = b1._invMass, m2 = b2._invMass;
float i1 = b1._invI, i2 = b2._invI;
_mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
_mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
_mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
_mass.Col1.Y = _mass.Col2.X;
_mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
_mass.Col3.Y = r1.X * i1 + r2.Y * i2;
_mass.Col1.Z = _mass.Col3.X;
_mass.Col2.Z = _mass.Col3.Y;
_mass.Col3.Z = i1 + i2;
_motorMass = 1.0f / (i1 + i2);
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (_enableLimit)
{
float jointAngle = b2._sweep.A - b1._sweep.A - _referenceAngle;
if (Box2DNetMath.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.EqualLimits;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLowerLimit)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtLowerLimit;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpperLimit)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtUpperLimit;
}
else
{
_limitState = LimitState.InactiveLimit;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
}
if (step.WarmStarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.DtRatio;
_motorImpulse *= step.DtRatio;
Vector2 P = _impulse.ToVector2();
b1._linearVelocity -= m1 * P;
b1._angularVelocity -= i1 * (r1.Cross(P) + _motorImpulse + _impulse.Z);
b2._linearVelocity += m2 * P;
b2._angularVelocity += i2 * (r2.Cross(P) + _motorImpulse + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
// Cdot = dot(u, v + cross(w, r))
MathUtils.Cross(b1.AngularVelocityInternal, ref r1, out _tmpVector1);
Vector2 v1 = b1.LinearVelocityInternal + _tmpVector1;
MathUtils.Cross(b2.AngularVelocityInternal, ref r2, out _tmpVector1);
Vector2 v2 = b2.LinearVelocityInternal + _tmpVector1;
float Cdot = Vector2.Dot(_u, v2 - v1);
float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
_impulse += impulse;
Vector2 P = impulse * _u;
b1.LinearVelocityInternal -= b1.InvMass * P;
MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
b1.AngularVelocityInternal -= b1.InvI * _tmpFloat1;
b2.LinearVelocityInternal += b2.InvMass * P;
MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
b2.AngularVelocityInternal += b2.InvI * _tmpFloat1;
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Vector2 v1 = b1.LinearVelocityInternal;
float w1 = b1.AngularVelocityInternal;
Vector2 v2 = b2.LinearVelocityInternal;
float w2 = b2.AngularVelocityInternal;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorForce;
_motorImpulse = MathHelper.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
Vector2 P = impulse * _axis;
float L1 = impulse * _a1;
float L2 = impulse * _a2;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1);
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint in block form.
float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 f1 = _impulse;
Vector3 df = _K.Solve33(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLower)
{
_impulse.Z = Math.Max(_impulse.Z, 0.0f);
}
else if (_limitState == LimitState.AtUpper)
{
_impulse.Z = Math.Min(_impulse.Z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.Col3.X, _K.Col3.Y);
Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y);
_impulse.X = f2r.X;
_impulse.Y = f2r.Y;
df = _impulse - f1;
Vector2 P = df.X * _perp + df.Z * _axis;
float L1 = df.X * _s1 + df.Y + df.Z * _a1;
float L2 = df.X * _s2 + df.Y + df.Z * _a2;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
Vector2 df = _K.Solve22(-Cdot1);
_impulse.X += df.X;
_impulse.Y += df.Y;
Vector2 P = df.X * _perp;
float L1 = df.X * _s1 + df.Y;
float L2 = df.X * _s2 + df.Y;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
b1.LinearVelocityInternal = v1;
b1.AngularVelocityInternal = w1;
b2.LinearVelocityInternal = v2;
b2.AngularVelocityInternal = w2;
}
/// <summary>
/// Updates the physics bodies and resolves all forces.
/// </summary>
private void Update(TimeStep timeStep)
{
ResolveMassiveBodyParents();
_spaceCraftManager.ResolveGravitionalParents(_massiveBodies);
AdjustSpeedForBurns(timeStep);
double targetDt = _isPaused ? 0 : timeStep.Dt;
// Update all bodies according to the timestep
for (int i = 0; i < timeStep.UpdateLoops; i++)
{
// Resolve n body massive body forces
foreach (IMassiveBody bodyA in _massiveBodies)
{
bodyA.ResetAccelerations();
foreach (IMassiveBody bodyB in _massiveBodies)
{
if (bodyA == bodyB)
{
continue;
}
bodyA.ResolveGravitation(bodyB);
}
}
_spaceCraftManager.ResolveForces(_massiveBodies);
_spaceCraftManager.Update(timeStep, targetDt);
// Update bodies
foreach (IGravitationalBody gravitationalBody in _gravitationalBodies)
{
gravitationalBody.Update(targetDt);
}
_totalElapsedSeconds += targetDt;
}
_camera.Update(TimeStep.RealTimeDt);
_pipCam.Update(TimeStep.RealTimeDt);
_eventManager.Update(TimeStep.RealTimeDt);
// Fixed update all gravitational bodies
foreach (IGravitationalBody body in _gravitationalBodies)
{
body.FixedUpdate(timeStep);
}
var targetSpaceCraft = _gravitationalBodies[_targetIndex] as ISpaceCraft;
if (targetSpaceCraft != null)
{
if (targetSpaceCraft.Controller.IsPrograde)
{
_progradeButton.Enable();
}
else
{
_progradeButton.Disable();
}
if (targetSpaceCraft.Controller.IsRetrograde)
{
_retrogradeButton.Enable();
}
else
{
_retrogradeButton.Disable();
}
}
_scrollRate = MathHelper.Lerp(_scrollRate, _targetScrollRate, 0.1f);
_targetScrollRate = MathHelper.Lerp(_targetScrollRate, 0, 0.1f);
if (_camera.Zoom > 1)
{
double scroll = Math.Pow(_camera.Zoom, 1.05f) * _scrollRate;
_camera.ChangeZoom(scroll);
}
else
{
_camera.ChangeZoom(_scrollRate);
}
SetCameraRotation();
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bB = BodyB;
LocalCenterA = Vector2.Zero;
LocalCenterB = bB.LocalCenter;
Transform xfB;
bB.GetTransform(out xfB);
// Compute the effective masses.
Vector2 rA = LocalAnchorA;
Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - LocalCenterB);
Vector2 d = bB.Sweep.C + rB - rA;
InvMassA = 0.0f;
InvIA = 0.0f;
InvMassB = bB.InvMass;
InvIB = bB.InvI;
// Point to line constraint
{
_ay = _localYAxisA;
_sAy = MathUtils.Cross(d + rA, _ay);
_sBy = MathUtils.Cross(rB, _ay);
_mass = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy;
if (_mass > 0.0f)
{
_mass = 1.0f / _mass;
}
}
// Spring constraint
_springMass = 0.0f;
if (Frequency > 0.0f)
{
_ax = LocalXAxis;
_sAx = MathUtils.Cross(d + rA, _ax);
_sBx = MathUtils.Cross(rB, _ax);
float invMass = InvMassA + InvMassB + InvIA * _sAx * _sAx + InvIB * _sBx * _sBx;
if (invMass > 0.0f)
{
_springMass = 1.0f / invMass;
float C = Vector2.Dot(d, _ax);
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
float da = 2.0f * _springMass * DampingRatio * omega;
// Spring stiffness
float k = _springMass * omega * omega;
// magic formulas
_gamma = step.dt * (da + step.dt * k);
if (_gamma > 0.0f)
{
_gamma = 1.0f / _gamma;
}
_bias = C * step.dt * k * _gamma;
_springMass = invMass + _gamma;
if (_springMass > 0.0f)
{
_springMass = 1.0f / _springMass;
}
}
}
else
{
_springImpulse = 0.0f;
_springMass = 0.0f;
}
// Rotational motor
if (_enableMotor)
{
_motorMass = InvIA + InvIB;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
}
else
{
_motorMass = 0.0f;
_motorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= step.dtRatio;
_springImpulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = _impulse * _ay + _springImpulse * _ax;
float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse;
bB.LinearVelocityInternal += InvMassB * P;
bB.AngularVelocityInternal += InvIB * LB;
}
else
{
_impulse = 0.0f;
_springImpulse = 0.0f;
_motorImpulse = 0.0f;
}
}
public void Reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities, bool warmstarting = Settings.EnableWarmstarting)
{
_step = step;
_count = count;
_positions = positions;
_velocities = velocities;
_contacts = contacts;
// grow the array
if (_velocityConstraints == null || _velocityConstraints.Length < count)
{
_velocityConstraints = new ContactVelocityConstraint[count * 2];
_positionConstraints = new ContactPositionConstraint[count * 2];
for (int i = 0; i < _velocityConstraints.Length; i++)
{
_velocityConstraints[i] = new ContactVelocityConstraint();
}
for (int i = 0; i < _positionConstraints.Length; i++)
{
_positionConstraints[i] = new ContactPositionConstraint();
}
}
// Initialize position independent portions of the constraints.
for (int i = 0; i < _count; ++i)
{
Contact contact = contacts[i];
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
Shape shapeA = fixtureA.Shape;
Shape shapeB = fixtureB.Shape;
float radiusA = shapeA.Radius;
float radiusB = shapeB.Radius;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
Manifold manifold = contact.Manifold;
int pointCount = manifold.PointCount;
Debug.Assert(pointCount > 0);
ContactVelocityConstraint vc = _velocityConstraints[i];
vc.friction = contact.Friction;
vc.restitution = contact.Restitution;
vc.tangentSpeed = contact.TangentSpeed;
vc.indexA = bodyA.IslandIndex;
vc.indexB = bodyB.IslandIndex;
vc.invMassA = bodyA._invMass;
vc.invMassB = bodyB._invMass;
vc.invIA = bodyA._invI;
vc.invIB = bodyB._invI;
vc.contactIndex = i;
vc.pointCount = pointCount;
vc.K.SetZero();
vc.normalMass.SetZero();
ContactPositionConstraint pc = _positionConstraints[i];
pc.indexA = bodyA.IslandIndex;
pc.indexB = bodyB.IslandIndex;
pc.invMassA = bodyA._invMass;
pc.invMassB = bodyB._invMass;
pc.localCenterA = bodyA._sweep.LocalCenter;
pc.localCenterB = bodyB._sweep.LocalCenter;
pc.invIA = bodyA._invI;
pc.invIB = bodyB._invI;
pc.localNormal = manifold.LocalNormal;
pc.localPoint = manifold.LocalPoint;
pc.pointCount = pointCount;
pc.radiusA = radiusA;
pc.radiusB = radiusB;
pc.type = manifold.Type;
for (int j = 0; j < pointCount; ++j)
{
ManifoldPoint cp = manifold.Points[j];
VelocityConstraintPoint vcp = vc.points[j];
if (Settings.EnableWarmstarting)
{
vcp.normalImpulse = _step.dtRatio * cp.NormalImpulse;
vcp.tangentImpulse = _step.dtRatio * cp.TangentImpulse;
}
else
{
vcp.normalImpulse = 0.0f;
vcp.tangentImpulse = 0.0f;
}
vcp.rA = Vector2.Zero;
vcp.rB = Vector2.Zero;
vcp.normalMass = 0.0f;
vcp.tangentMass = 0.0f;
vcp.velocityBias = 0.0f;
pc.localPoints[j] = cp.LocalPoint;
}
}
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
Vector2 p1 = b1._sweep.C + r1;
Vector2 p2 = b2._sweep.C + r2;
Vector2 s1 = _ground.GetTransform().position + _groundAnchor1;
Vector2 s2 = _ground.GetTransform().position + _groundAnchor2;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.Length();
float length2 = _u2.Length();
if (length1 > Settings.LinearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.Zero;
}
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _constant - length1 - _ratio * length2;
if (C > 0.0f)
{
_state = LimitState.InactiveLimit;
_impulse = 0.0f;
}
else
{
_state = LimitState.AtUpperLimit;
}
if (length1 < _maxLength1)
{
_limitState1 = LimitState.InactiveLimit;
_limitImpulse1 = 0.0f;
}
else
{
_limitState1 = LimitState.AtUpperLimit;
}
if (length2 < _maxLength2)
{
_limitState2 = LimitState.InactiveLimit;
_limitImpulse2 = 0.0f;
}
else
{
_limitState2 = LimitState.AtUpperLimit;
}
// Compute effective mass.
float cr1u1 = r1.Cross(_u1);
float cr2u2 = r2.Cross(_u2);
_limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1;
_limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2;
_pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2;
Box2DNetDebug.Assert(_limitMass1 > Settings.FLT_EPSILON);
Box2DNetDebug.Assert(_limitMass2 > Settings.FLT_EPSILON);
Box2DNetDebug.Assert(_pulleyMass > Settings.FLT_EPSILON);
_limitMass1 = 1.0f / _limitMass1;
_limitMass2 = 1.0f / _limitMass2;
_pulleyMass = 1.0f / _pulleyMass;
if (step.WarmStarting)
{
// Scale impulses to support variable time steps.
_impulse *= step.DtRatio;
_limitImpulse1 *= step.DtRatio;
_limitImpulse2 *= step.DtRatio;
// Warm starting.
Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
Vector2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * r1.Cross(P1);
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * r2.Cross(P2);
}
else
{
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vector2 v1 = b1._linearVelocity;
float w1 = b1._angularVelocity;
Vector2 v2 = b2._linearVelocity;
float w2 = b2._angularVelocity;
float m1 = b1._invMass, m2 = b2._invMass;
float i1 = b1._invI, i2 = b2._invI;
//Solve motor constraint.
if (_enableMotor && _limitState != LimitState.EqualLimits)
{
float Cdot = w2 - w1 - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.Dt * _maxMotorTorque;
_motorImpulse = Box2DNet.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
w1 -= i1 * impulse;
w2 += i2 * impulse;
}
//Solve limit constraint.
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
// Solve point-to-point constraint
Vector2 Cdot1 = v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1);
float Cdot2 = w2 - w1;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 impulse = _mass.Solve33(-Cdot);
if (_limitState == LimitState.EqualLimits)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLowerLimit)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
else if (_limitState == LimitState.AtUpperLimit)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
Vector2 P = impulse.ToVector2();
v1 -= m1 * P;
w1 -= i1 * (r1.Cross(P) + impulse.Z);
v2 += m2 * P;
w2 += i2 * (r2.Cross(P) + impulse.Z);
}
else
{
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
// Solve point-to-point constraint
Vector2 Cdot = v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1);
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.X += impulse.X;
_impulse.Y += impulse.Y;
v1 -= m1 * impulse;
w1 -= i1 * r1.Cross(impulse);
v2 += m2 * impulse;
w2 += i2 * r2.Cross(impulse);
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = w2;
}
// djm pooling
public void init(ContactSolverDef def)
{
// System.out.println("Initializing contact solver");
m_step = def.step;
m_count = def.count;
if (m_positionConstraints.Length < m_count)
{
ContactPositionConstraint[] old = m_positionConstraints;
m_positionConstraints = new ContactPositionConstraint[MathUtils.max(old.Length * 2, m_count)];
ArrayHelper.Copy(old, 0, m_positionConstraints, 0, old.Length);
for (int i = old.Length; i < m_positionConstraints.Length; i++)
{
m_positionConstraints[i] = new ContactPositionConstraint();
}
}
if (m_velocityConstraints.Length < m_count)
{
ContactVelocityConstraint[] old = m_velocityConstraints;
m_velocityConstraints = new ContactVelocityConstraint[MathUtils.max(old.Length * 2, m_count)];
ArrayHelper.Copy(old, 0, m_velocityConstraints, 0, old.Length);
for (int i = old.Length; i < m_velocityConstraints.Length; i++)
{
m_velocityConstraints[i] = new ContactVelocityConstraint();
}
}
m_positions = def.positions;
m_velocities = def.velocities;
m_contacts = def.contacts;
for (int i = 0; i < m_count; ++i)
{
// System.out.println("contacts: " + m_count);
Contact contact = m_contacts[i];
Fixture fixtureA = contact.m_fixtureA;
Fixture fixtureB = contact.m_fixtureB;
Shape shapeA = fixtureA.getShape();
Shape shapeB = fixtureB.getShape();
double radiusA = shapeA.m_radius;
double radiusB = shapeB.m_radius;
Body bodyA = fixtureA.getBody();
Body bodyB = fixtureB.getBody();
Manifold manifold = contact.getManifold();
int pointCount = manifold.pointCount;
ContactVelocityConstraint vc = m_velocityConstraints[i];
vc.friction = contact.m_friction;
vc.restitution = contact.m_restitution;
vc.tangentSpeed = contact.m_tangentSpeed;
vc.indexA = bodyA.m_islandIndex;
vc.indexB = bodyB.m_islandIndex;
vc.invMassA = bodyA.m_invMass;
vc.invMassB = bodyB.m_invMass;
vc.invIA = bodyA.m_invI;
vc.invIB = bodyB.m_invI;
vc.contactIndex = i;
vc.pointCount = pointCount;
vc.K.setZero();
vc.normalMass.setZero();
ContactPositionConstraint pc = m_positionConstraints[i];
pc.indexA = bodyA.m_islandIndex;
pc.indexB = bodyB.m_islandIndex;
pc.invMassA = bodyA.m_invMass;
pc.invMassB = bodyB.m_invMass;
pc.localCenterA.set(bodyA.m_sweep.localCenter);
pc.localCenterB.set(bodyB.m_sweep.localCenter);
pc.invIA = bodyA.m_invI;
pc.invIB = bodyB.m_invI;
pc.localNormal.set(manifold.localNormal);
pc.localPoint.set(manifold.localPoint);
pc.pointCount = pointCount;
pc.radiusA = radiusA;
pc.radiusB = radiusB;
pc.type = manifold.type;
// System.out.println("contact point count: " + pointCount);
for (int j = 0; j < pointCount; j++)
{
ManifoldPoint cp = manifold.points[j];
VelocityConstraintPoint vcp = vc.points[j];
if (m_step.warmStarting)
{
// assert(cp.normalImpulse == 0);
// System.out.println("contact normal impulse: " + cp.normalImpulse);
vcp.normalImpulse = m_step.dtRatio * cp.normalImpulse;
vcp.tangentImpulse = m_step.dtRatio * cp.tangentImpulse;
}
else
{
vcp.normalImpulse = 0;
vcp.tangentImpulse = 0;
}
vcp.rA.setZero();
vcp.rB.setZero();
vcp.normalMass = 0;
vcp.tangentMass = 0;
vcp.velocityBias = 0;
pc.localPoints[j].x = cp.localPoint.x;
pc.localPoints[j].y = cp.localPoint.y;
}
}
}
public override void Detect(TimeStep step)
{
this.step = step;
this.Run();
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
LocalCenterA = b1.LocalCenter;
LocalCenterB = b2.LocalCenter;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
// Compute the effective masses.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - LocalCenterA);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - LocalCenterB);
Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
InvMassA = b1.InvMass;
InvIA = b1.InvI;
InvMassB = b2.InvMass;
InvIB = b2.InvI;
// Compute motor Jacobian and effective mass.
{
_axis = MathUtils.Multiply(ref xf1.R, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
_motorMass = InvMassA + InvMassB + InvIA * _a1 * _a1 + InvIB * _a2 * _a2;
if (_motorMass > Settings.Epsilon)
{
_motorMass = 1.0f / _motorMass;
}
}
// Prismatic constraint.
{
_perp = MathUtils.Multiply(ref xf1.R, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector3(k11, k12, k13);
_K.Col2 = new Vector3(k12, k22, k23);
_K.Col3 = new Vector3(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis;
float L1 = _impulse.X * _s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a1;
float L2 = _impulse.X * _s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a2;
b1.LinearVelocityInternal -= InvMassA * P;
b1.AngularVelocityInternal -= InvIA * L1;
b2.LinearVelocityInternal += InvMassB * P;
b2.AngularVelocityInternal += InvIB * L2;
}
else
{
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
}
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
// Compute the effective mass matrix.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
_u = b2.Sweep.C + r2 - b1.Sweep.C - r1;
// Handle singularity.
float length = _u.Length();
if (length < MaxLength && length > MinLength)
{
return;
}
if (length > Settings.LinearSlop)
{
_u *= 1.0f / length;
}
else
{
_u = Vector2.Zero;
}
float cr1u = MathUtils.Cross(r1, _u);
float cr2u = MathUtils.Cross(r2, _u);
float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + b2.InvMass + b2.InvI * cr2u * cr2u;
Debug.Assert(invMass > Settings.Epsilon);
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (Frequency > 0.0f)
{
float C = length - MaxLength;
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
float d = 2.0f * _mass * DampingRatio * omega;
// Spring stiffness
float k = _mass * omega * omega;
// magic formulas
_gamma = step.dt * (d + step.dt * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * step.dt * k * _gamma;
_mass = invMass + _gamma;
_mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale the impulse to support a variable time step.
_impulse *= step.dtRatio;
Vector2 P = _impulse * _u;
b1.LinearVelocityInternal -= b1.InvMass * P;
b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
b2.LinearVelocityInternal += b2.InvMass * P;
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P);
}
else
{
_impulse = 0.0f;
}
}
public void RunOneStep(TimeStep timestep, DateTime now, bool runProcessing)
{
/*if (_allProfiles == null) {
* throw new LPGException("all profiles was null");
* }*/
if (_households == null)
{
throw new LPGException("households was null");
}
if (_autoDevs == null)
{
throw new LPGException("_autoDevs was null");
}
if (_transformationDevices == null)
{
throw new LPGException("_transformationDevices was null");
}
if (_energyStorages == null)
{
throw new LPGException("_energyStorages was null");
}
if (_generators == null)
{
throw new LPGException("_generators was null");
}
foreach (var household in _households)
{
household.RunOneStep(timestep, now, false);
}
if (_calcSpaceHeating != null)
{
if (!_calcSpaceHeating.IsBusyDuringTimespan(timestep, 1, 1, _calcSpaceHeating.Loads[0].LoadType))
{
_calcSpaceHeating.Activate(timestep, now);
}
}
if (_calcAirConditioning?.IsBusyDuringTimespan(timestep, 1, 1,
_calcAirConditioning.Loads[0].LoadType) == false)
{
_calcAirConditioning.Activate(timestep, now);
}
foreach (var calcAutoDev in _autoDevs)
{
if (!calcAutoDev.IsBusyDuringTimespan(timestep, 1, 1, calcAutoDev.LoadType))
{
calcAutoDev.Activate(timestep);
}
}
var runAgain = true;
var energyFileRows = _calcRepo.Odap.ProcessOneTimestep(timestep);
var repetitioncount = 0;
List <string> log = null;
while (runAgain)
{
runAgain = false;
repetitioncount++;
if (repetitioncount == 98)
{
log = new List <string>();
}
if (repetitioncount > 100)
{
var builder = new StringBuilder();
foreach (var transformationDevice in _transformationDevices)
{
builder.Append(transformationDevice.Name).Append(Environment.NewLine);
}
foreach (var dev in _energyStorages)
{
builder.Append(dev.Name).Append(Environment.NewLine);
}
foreach (var dev in _generators)
{
builder.Append(dev.Name).Append(Environment.NewLine);
}
var protocol = string.Empty;
if (log != null)
{
foreach (var s1 in log)
{
protocol += s1 + Environment.NewLine;
}
}
throw new DataIntegrityException(
"Did more than 100 tries while trying to calculate the transformation devices in the house " +
Name + " without reaching a solution. " +
"This most likely means you have a transformation device loop which is not permitted. " +
"A loop might be device A makes water from electricity and device B makes electricty from water. " +
"The devices are:" + Environment.NewLine + builder +
Environment.NewLine + Environment.NewLine + "The last actions were:" + Environment.NewLine +
protocol);
}
foreach (var transformationDevice in _transformationDevices)
{
if (transformationDevice.ProcessOneTimestep(energyFileRows, log))
{
runAgain = true;
}
}
foreach (var energyStorage in _energyStorages)
{
if (energyStorage.ProcessOneTimestep(energyFileRows, timestep, log))
{
runAgain = true;
}
}
foreach (var calcGenerator in _generators)
{
if (calcGenerator.ProcessOneTimestep(energyFileRows, timestep, log))
{
runAgain = true;
}
}
}
foreach (var fileRow in energyFileRows)
{
if (_calcRepo.CalcParameters.IsSet(CalcOption.DetailedDatFiles))
{
fileRow.Save(_calcRepo.Odap.BinaryOutStreams[fileRow.LoadType]);
}
if (_calcRepo.CalcParameters.IsSet(CalcOption.OverallDats))
{
fileRow.SaveSum(_calcRepo.Odap.SumBinaryOutStreams[fileRow.LoadType]);
}
}
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
// Compute the effective mass matrix.
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = bA.InvMass, mB = bB.InvMass;
float iA = bA.InvI, iB = bB.InvI;
Mat22 K1 = new Mat22();
K1.Col1.X = mA + mB;
K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f;
K1.Col2.Y = mA + mB;
Mat22 K2 = new Mat22();
K2.Col1.X = iA * rA.Y * rA.Y;
K2.Col2.X = -iA * rA.X * rA.Y;
K2.Col1.Y = -iA * rA.X * rA.Y;
K2.Col2.Y = iA * rA.X * rA.X;
Mat22 K3 = new Mat22();
K3.Col1.X = iB * rB.Y * rB.Y;
K3.Col2.X = -iB * rB.X * rB.Y;
K3.Col1.Y = -iB * rB.X * rB.Y;
K3.Col2.Y = iB * rB.X * rB.X;
Mat22 K12;
Mat22.Add(ref K1, ref K2, out K12);
Mat22 K;
Mat22.Add(ref K12, ref K3, out K);
_linearMass = K.Inverse;
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= step.dtRatio;
_angularImpulse *= step.dtRatio;
Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
bA.LinearVelocityInternal -= mA * P;
bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);
bB.LinearVelocityInternal += mB * P;
bB.AngularVelocityInternal += iB * (MathUtils.Cross(rB, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.Zero;
_angularImpulse = 0.0f;
}
}
public SharedDesireValue(decimal currentValue, [CanBeNull] TimeStep lastDecay)
{
CurrentValue = currentValue;
LastDecay = lastDecay;
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Vector2 vA = bA.LinearVelocityInternal;
float wA = bA.AngularVelocityInternal;
Vector2 vB = bB.LinearVelocityInternal;
float wB = bB.AngularVelocityInternal;
float mA = bA.InvMass, mB = bB.InvMass;
float iA = bA.InvI, iB = bB.InvI;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
// Solve angular friction
{
float Cdot = wB - wA;
float impulse = -_angularMass * Cdot;
float oldImpulse = _angularImpulse;
float maxImpulse = step.dt * MaxTorque;
_angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
Vector2 Cdot = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);
Vector2 impulse = -MathUtils.Multiply(ref _linearMass, Cdot);
Vector2 oldImpulse = _linearImpulse;
_linearImpulse += impulse;
float maxImpulse = step.dt * MaxForce;
if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
_linearImpulse.Normalize();
_linearImpulse *= maxImpulse;
}
impulse = _linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(rA, impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(rB, impulse);
}
bA.LinearVelocityInternal = vA;
bA.AngularVelocityInternal = wA;
bB.LinearVelocityInternal = vB;
bB.AngularVelocityInternal = wB;
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Vector2 vA = bA.LinearVelocityInternal;
float wA = bA.AngularVelocityInternal;
Vector2 vB = bB.LinearVelocityInternal;
float wB = bB.AngularVelocityInternal;
float mA = bA.InvMass, mB = bB.InvMass;
float iA = bA.InvI, iB = bB.InvI;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
// Solve point-to-point constraint
Vector2 Cdot1 = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);
float Cdot2 = wB - wA;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 impulse = _mass.Solve33(-Cdot);
_impulse += impulse;
Vector2 P = new Vector2(impulse.X, impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(rA, P) + impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(rB, P) + impulse.Z);
bA.LinearVelocityInternal = vA;
bA.AngularVelocityInternal = wA;
bB.LinearVelocityInternal = vB;
bB.AngularVelocityInternal = wB;
}
public override void FixedUpdate(TimeStep timeStep)
{
OrbitHelper.TraceMassiveBody(this, OrbitTrace);
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
// Cdot = dot(u, v + cross(w, r))
Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
Vector2 v2 = new Vector2(0, 0);
float Cdot = Vector2.Dot(_u, v2 - v1);
float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
_impulse += impulse;
Vector2 P = impulse * _u;
b1.LinearVelocityInternal -= b1.InvMass * P;
b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
}
protected internal virtual void UpdateTime(TimeStep step)
{
this.lifetime.Update(step);
}
/// <summary>
/// Fills a cell with appropriate brush based on its data, and writes variable names if under variable control.
/// </summary>
private void refreshCell(Graphics g, Point p)
{
DigitalDataPoint dp = cellPointToDigitalDataPoint(p);
Pen outlinePen = normalOutlinePen;
/* if (dp != null)
* {
* if (dp.usesPulse())
* {
* outlinePen = pulseOutlintPen;
* }
* else
* {
* outlinePen = normalOutlinePen;
* }
* }*/
if (dp == null)
{
paintCell(g, p, nullBrush, outlinePen);
return;
}
if (dp.variable == null)
{
Brush br;
if (dp.DigitalContinue)
{
int stepID = cellPointToTimeStepId(p);
int channelID = cellPointToChannelID(p);
if (stepID == -1 || channelID == -1)
{
return;
}
TimeStep step = Storage.sequenceData.TimeSteps[stepID];
bool continueValue = step.getDigitalValue(channelID,
Storage.sequenceData.TimeSteps,
stepID);
br = continueBrush(p.Y, continueValue);
}
else if (dp.ManualValue)
{
br = trueBrush(p.Y);
// paintCell(g, p, trueBrush(p.Y), outlinePen);
}
else
{
br = falseBrush;
// paintCell(g, p, falseBrush, outlinePen);
//painCellRectInternal(g, p, new Pen(trueBrush(p.Y)));
//drawCellTopAndBottomInternalLines(g, p, new Pen(trueBrush(p.Y)));
}
paintCell(g, p, br, outlinePen);
}
else
{
// this will probably never get used. Though there is functionality for having a variable value for a digital
// this does not get set anywhere. Probably this is superseeded by pulse capability
paintCell(g, p, variableBrush, outlinePen);
g.DrawString(dp.variable.ToString(), variableFont, Brushes.Black, new RectangleF(p.X * colWidth, p.Y * rowHeight, colWidth, rowHeight));
}
if (dp.DigitalPulse != null)
{
p = paintPulse(g, p, dp);
}
if (WordGenerator.MainClientForm.instance.studentEdition)
{
paintCell(g, p, seb, null);
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
if (_state == LimitState.AtUpper)
{
Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
float Cdot = -Vector2.Dot(_u1, v1) - Ratio * Vector2.Dot(_u2, v2);
float impulse = _pulleyMass * (-Cdot);
float oldImpulse = _impulse;
_impulse = Math.Max(0.0f, _impulse + impulse);
impulse = _impulse - oldImpulse;
Vector2 P1 = -impulse * _u1;
Vector2 P2 = -Ratio * impulse * _u2;
b1.LinearVelocityInternal += b1.InvMass * P1;
b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
b2.LinearVelocityInternal += b2.InvMass * P2;
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
}
if (_limitState1 == LimitState.AtUpper)
{
Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
float Cdot = -Vector2.Dot(_u1, v1);
float impulse = -_limitMass1 * Cdot;
float oldImpulse = _limitImpulse1;
_limitImpulse1 = Math.Max(0.0f, _limitImpulse1 + impulse);
impulse = _limitImpulse1 - oldImpulse;
Vector2 P1 = -impulse * _u1;
b1.LinearVelocityInternal += b1.InvMass * P1;
b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
}
if (_limitState2 == LimitState.AtUpper)
{
Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
float Cdot = -Vector2.Dot(_u2, v2);
float impulse = -_limitMass2 * Cdot;
float oldImpulse = _limitImpulse2;
_limitImpulse2 = Math.Max(0.0f, _limitImpulse2 + impulse);
impulse = _limitImpulse2 - oldImpulse;
Vector2 P2 = -impulse * _u2;
b2.LinearVelocityInternal += b2.InvMass * P2;
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
}
}
/// <summary>
/// Draws all the physics bodies and UI elements.
/// </summary>
private unsafe void DrawFrame(TimeStep timeStep, FpsManager frameTimer)
{
_textDisplay.Clear();
// check for global events
_eventManager.CheckForGlobalEvents(this);
RectangleD cameraBounds = _camera.Bounds;
IGravitationalBody target = _gravitationalBodies[_targetIndex];
var targetSpaceCraft = target as SpaceCraftBase;
// If openCL is supported render all cl bodies
if (_renderingType == RenderingType.OpenCLHardware ||
_renderingType == RenderingType.OpenCLSoftware)
{
_gpuClear.RenderCl(_clProxy);
foreach (MassiveBodyBase renderable in _massiveBodies)
{
if (renderable.Visibility(cameraBounds) > 0)
{
renderable.RenderCl(_clProxy, _camera, _sun);
}
}
int[] frameData = _clProxy.ReadIntBuffer("image", RenderUtils.ScreenArea);
var rect = new Rectangle(0, 0, _imageBitmap.Width, _imageBitmap.Height);
BitmapData bmpData = _imageBitmap.LockBits(rect, ImageLockMode.WriteOnly,
PixelFormat.Format32bppArgb);
Marshal.Copy(frameData, 0, bmpData.Scan0, RenderUtils.ScreenArea);
var ptr = (byte *)bmpData.Scan0;
// Hack to force full alpha for now
for (int i = 0; i < RenderUtils.ScreenArea; i++)
{
ptr[i * 4 + 3] = 255;
}
_imageBitmap.UnlockBits(bmpData);
}
else
{
// Fall back to gdi for cl renderables
using (var graphics = Graphics.FromImage(_imageBitmap))
{
graphics.Clear(Color.Black);
_camera.ApplyScreenRotation(graphics);
foreach (MassiveBodyBase renderable in _massiveBodies)
{
if (renderable.Visibility(cameraBounds) > 0)
{
renderable.RenderGdiFallback(graphics, _camera, _sun);
}
}
}
}
// Draw all orbit traces, spacecrafts, and GDI objects
using (Graphics graphics = RenderUtils.GetContext(false, _imageBitmap))
{
_camera.ApplyScreenRotation(graphics);
// Draw all massive body orbit traces
foreach (MassiveBodyBase massiveBody in _massiveBodies)
{
if (massiveBody is Sun)
{
continue;
}
massiveBody.RenderGdi(graphics, _camera);
}
graphics.ResetTransform();
// Draw structures
foreach (StructureBase structure in _structures)
{
structure.RenderGdi(graphics, _camera);
}
_spaceCraftManager.Render(graphics, _camera);
}
// Draw all GUI elements (higher quality)
using (Graphics graphics = RenderUtils.GetContext(true, _imageBitmap))
{
double throttle = 0;
if (targetSpaceCraft != null)
{
throttle = targetSpaceCraft.Throttle;
// TODO: render PIP
//int width = RenderUtils.ScreenWidth;
//int height = RenderUtils.ScreenHeight;
//Rectangle rectPip = new Rectangle(width - 220, 100, 200, height - 300);
//graphics.DrawRectangle(Pens.White, rectPip);
//_pipCam.ApplyScreenRotation(graphics);
//_spaceCraftManager.Render(graphics, _pipCam);
}
double pitch = 0.0;
double flightPathAngle = 0.0;
foreach (IGauge gauge in _gauges)
{
if (targetSpaceCraft != null)
{
if (_targetInOrbit && _rotateInOrbit)
{
pitch = _gravitationalBodies[_targetIndex].Pitch;
}
else
{
pitch = _gravitationalBodies[_targetIndex].GetRelativePitch();
}
flightPathAngle = pitch - targetSpaceCraft.GetAlpha();
gauge.Update(pitch, throttle / 100.0, flightPathAngle);
}
gauge.Render(graphics, cameraBounds);
}
_eventManager.Render(graphics);
var elapsedTime = TimeSpan.FromSeconds(_totalElapsedSeconds - _clockDelay);
int elapsedYears = elapsedTime.Days / 365;
int elapsedDays = elapsedTime.Days % 365;
DateTime localTime = _originTime + elapsedTime;
// Main timing display
_textDisplay.AddTextBlock(StringAlignment.Near, new List <string>
{
//$"Origin Time: {localTime.ToShortDateString()} {localTime.ToShortTimeString()}",
$"Origin Time: {string.Format("{0}-{1}-{2:D2}", localTime.Date.Year, localTime.Date.Month, localTime.Date.Day)} {localTime.ToShortTimeString()}",
//$"Elapsed Time: Y:{elapsedYears} D:{elapsedDays} H:{elapsedTime.Hours} M:{elapsedTime.Minutes} S:{Math.Round(elapsedTime.TotalSeconds % 60)}",
$"Elapsed Time: Y:{elapsedYears} D:{elapsedDays} H:{elapsedTime.Hours} M:{elapsedTime.Minutes} S:{elapsedTime.Seconds}",
$"Update Speed: {timeStep.Multiplier}"
});
// Target display
_textDisplay.AddTextBlock(StringAlignment.Center, string.Format("Target: {0}", target));
// FPS
_textDisplay.AddTextBlock(StringAlignment.Far, "FPS: " + frameTimer.CurrentFps);
double targetVelocity = target.GetRelativeVelocity().Length();
double inertialVelocity = target.GetInertialVelocity().Length();
// Info for altitude
// double altitude = target.GetRelativeAltitude();
// double altitude = target.GetRelativeAltitude() + 82.5; // Starship Mk1
double altitude = target.GetRelativeAltitude() - 111.4; // Starship + Super Heavy
var altitudeInfo = new List <string> {
"Altitude: " + UnitDisplay.Distance(altitude)
};
// Add downrange if spacecraft exists
if (targetSpaceCraft != null)
{
double downrangeDistance = targetSpaceCraft.GetDownrangeDistance(_structures[0].Position);
altitudeInfo.Add("Downrange: " + UnitDisplay.Distance(downrangeDistance));
}
_textDisplay.AddTextBlock(StringAlignment.Near, altitudeInfo);
// Info for speed / acceleration
var movementInfo = new List <string>
{
"Inertial Velocity: " + UnitDisplay.Speed(targetVelocity, useKmh),
"Orbital Velocity: " + UnitDisplay.Speed(inertialVelocity, useKmh),
"Inertial Acceleration: " + UnitDisplay.Acceleration(target.GetRelativeAcceleration().Length()),
"Orbital Acceleration: " + UnitDisplay.Acceleration(target.GetInertialAcceleration().Length())
};
double lateralVelocity = target.GetLateralVelocity().Length();
if (lateralVelocity > 0)
{
double lateralPosition = target.GetLateralPosition().Length();
altitudeInfo.Add("Crossrange: " + UnitDisplay.Distance(lateralPosition));
movementInfo.Add("Lateral Velocity: " + UnitDisplay.Speed(lateralVelocity, useKmh));
movementInfo.Add("Lateral Acceleration: " + UnitDisplay.Acceleration(target.GetLateralAcceleration().Length()));
}
// Add angle of attack if it exists
if (targetSpaceCraft != null)
{
movementInfo.Add("Angle of Attack: " + UnitDisplay.AoA(targetSpaceCraft.GetAlpha()));
// calculate the current inclination
double G = 6.67408E-11;
double M = target.GravitationalParent.Mass;
double μ = G * M;
double R = target.GravitationalParent.SurfaceRadius;
double apogee = target.Apoapsis;
if (initialPerigee == 0)
{
initialPerigee = target.Periapsis;
}
double Ra = R + apogee;
double Rp = R + initialPerigee;
double e = 1 - 2 / ((Ra / Rp) + 1);
double ω = 0;
double f = 0;
// orbital period
double a = (initialPerigee + R * 2 + apogee) / 2;
double P = 2 * Math.PI * Math.Pow(Math.Pow(a, 3) / μ, 0.5);
double n = 2 * Math.PI / P;
double Δi = 2 * Math.Asin(lateralVelocity * (1 + e * Math.Cos(f)) / (Math.Pow(1 - Math.Pow(e, 2), 0.5) * Math.Cos(ω + f) * n * a * 2));
double inclination = launchInclination * MathHelper.DegreesToRadians - Δi;
movementInfo.Add("Inclination: " + UnitDisplay.Degrees(inclination));
double speedOfSound = targetSpaceCraft.GravitationalParent.GetSpeedOfSound(target.GetRelativeAltitude());
if (speedOfSound > 0)
{
double machNumber = targetVelocity / speedOfSound;
movementInfo.Add("Mach number: " + UnitDisplay.Mach(machNumber));
}
}
_textDisplay.AddTextBlock(StringAlignment.Near, movementInfo);
var forceInfo = new List <string> {
"Mass: " + UnitDisplay.Mass(target.Mass)
};
// Add additional forces
if (targetSpaceCraft != null)
{
DVector2 dragForce = targetSpaceCraft.AccelerationD * targetSpaceCraft.Mass;
DVector2 liftForce = targetSpaceCraft.AccelerationL * targetSpaceCraft.Mass;
if (Settings.Default.UseTheTurnForce)
{
liftForce *= Math.Cos(targetSpaceCraft.Roll);
}
forceInfo.Add("Thrust: " + UnitDisplay.Force(targetSpaceCraft.Thrust));
forceInfo.Add("Drag: " + UnitDisplay.Force(dragForce.Length()));
forceInfo.Add("Lift: " + UnitDisplay.Force(liftForce.Length()));
}
_textDisplay.AddTextBlock(StringAlignment.Near, forceInfo);
// Don't show Apoapsis/Periapsis info for the sun
if (!(target is Sun) && target.GravitationalParent != null)
{
_textDisplay.AddTextBlock(StringAlignment.Near, new List <string>
{
$"{target.GravitationalParent.ApoapsisName}: {UnitDisplay.Distance(target.Apoapsis)}",
$"{target.GravitationalParent.PeriapsisName}: {UnitDisplay.Distance(target.Periapsis)}",
});
}
// Add atmospheric info if the spaceship is the target
if (targetSpaceCraft != null && targetSpaceCraft.GravitationalParent != null)
{
double density = targetSpaceCraft.GravitationalParent.GetAtmosphericDensity(target.GetRelativeAltitude());
double dynamicPressure = 0.5 * density * targetVelocity * targetVelocity;
_textDisplay.AddTextBlock(StringAlignment.Near, new List <string>
{
"Air Density: " + UnitDisplay.Density(density),
"Dynamic Pressure: " + UnitDisplay.Pressure(dynamicPressure),
"Heat Flux: " + UnitDisplay.Heat(targetSpaceCraft.HeatingRate)
});
}
_textDisplay.Draw(graphics);
}
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
// Compute the effective mass matrix.
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
_u = b2._sweep.C + r2 - b1._sweep.C - r1;
// Handle singularity.
float length = _u.Length();
if (length > Settings.LinearSlop)
{
_u *= 1.0f / length;
}
else
{
_u = Vector2.Zero;
}
float cr1u = r1.Cross(_u);
float cr2u = r2.Cross(_u);
float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;
_mass = 1.0f / invMass;
if (_frequencyHz > 0.0f)
{
float C = length - _length;
// Frequency
float omega = 2.0f * Settings.Pi * _frequencyHz;
// Damping coefficient
float d = 2.0f * _mass * _dampingRatio * omega;
// Spring stiffness
float k = _mass * omega * omega;
// magic formulas
_gamma = 1.0f / (step.Dt * (d + step.Dt * k));
_bias = C * step.Dt * k * _gamma;
_mass = 1.0f / (invMass + _gamma);
}
if (step.WarmStarting)
{
//Scale the inpulse to support a variable timestep.
_impulse *= step.DtRatio;
Vector2 P = _impulse * _u;
b1._linearVelocity -= b1._invMass * P;
b1._angularVelocity -= b1._invI * r1.Cross(P);
b2._linearVelocity += b2._invMass * P;
b2._angularVelocity += b2._invI * r2.Cross(P);
}
else
{
_impulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bB = BodyB;
Vector2 vA = Vector2.Zero;
float wA = 0.0f;
Vector2 vB = bB.LinearVelocityInternal;
float wB = bB.AngularVelocityInternal;
// Solve spring constraint
{
float Cdot = Vector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA;
float impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse);
_springImpulse += impulse;
Vector2 P = impulse * _ax;
float LA = impulse * _sAx;
float LB = impulse * _sBx;
vA -= InvMassA * P;
wA -= InvIA * LA;
vB += InvMassB * P;
wB += InvIB * LB;
}
// Solve rotational motor constraint
{
float Cdot = wB - wA - _motorSpeed;
float impulse = -_motorMass * Cdot;
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorTorque;
_motorImpulse = MathHelper.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
wA -= InvIA * impulse;
wB += InvIB * impulse;
}
// Solve point to line constraint
{
float Cdot = Vector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA;
float impulse = _mass * (-Cdot);
_impulse += impulse;
Vector2 P = impulse * _ay;
float LB = impulse * _sBy;
vB += InvMassB * P;
wB += InvIB * LB;
}
bB.LinearVelocityInternal = vB;
bB.AngularVelocityInternal = wB;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
if (_enableMotor || _enableLimit)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
Debug.Assert(b1.InvI > 0.0f /* || b2._invI > 0.0f*/);
}
// Compute the effective mass matrix.
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = LocalAnchorB; // MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2]
// [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2]
// [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2]
float m1 = b1.InvMass;
const float m2 = 0;
float i1 = b1.InvI;
const float i2 = 0;
_mass.col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
_mass.col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
_mass.col3.X = -r1.Y * i1 - r2.Y * i2;
_mass.col1.Y = _mass.col2.X;
_mass.col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
_mass.col3.Y = r1.X * i1 + r2.X * i2;
_mass.col1.Z = _mass.col3.X;
_mass.col2.Z = _mass.col3.Y;
_mass.col3.Z = i1 + i2;
_motorMass = i1 + i2;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (_enableLimit)
{
float jointAngle = 0 - b1.Sweep.a - ReferenceAngle;
if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.Equal;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLower)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtLower;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpper)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtUpper;
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
b1.LinearVelocityInternal -= m1 * P;
b1.AngularVelocityInternal -= i1 * (MathUtils.Cross(r1, P) + _motorImpulse + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
}
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
// Compute the effective mass matrix.
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = bA.InvMass, mB = bB.InvMass;
float iA = bA.InvI, iB = bB.InvI;
_mass.Col1.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
_mass.Col2.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
_mass.Col3.X = -rA.Y * iA - rB.Y * iB;
_mass.Col1.Y = _mass.Col2.X;
_mass.Col2.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
_mass.Col3.Y = rA.X * iA + rB.X * iB;
_mass.Col1.Z = _mass.Col3.X;
_mass.Col2.Z = _mass.Col3.Y;
_mass.Col3.Z = iA + iB;
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.dtRatio;
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
bA.LinearVelocityInternal -= mA * P;
bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _impulse.Z);
bB.LinearVelocityInternal += mB * P;
bB.AngularVelocityInternal += iB * (MathUtils.Cross(rB, P) + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Vector2 v1 = b1.LinearVelocityInternal;
float w1 = b1.AngularVelocityInternal;
Vector2 v2 = Vector2.Zero;
const float w2 = 0;
float m1 = b1.InvMass;
float i1 = b1.InvI;
// Solve motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = w2 - w1 - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
w1 -= i1 * impulse;
}
// Solve limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = LocalAnchorB;
// Solve point-to-point constraint
Vector2 Cdot1 = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
float Cdot2 = w2 - w1;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 impulse = _mass.Solve33(-Cdot);
if (_limitState == LimitState.Equal)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLower)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
else if (_limitState == LimitState.AtUpper)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
Vector2 P = new Vector2(impulse.X, impulse.Y);
v1 -= m1 * P;
w1 -= i1 * (MathUtils.Cross(r1, P) + impulse.Z);
}
else
{
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = LocalAnchorB;
// Solve point-to-point constraint
Vector2 Cdot = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.X += impulse.X;
_impulse.Y += impulse.Y;
v1 -= m1 * impulse;
w1 -= i1 * MathUtils.Cross(r1, impulse);
}
b1.LinearVelocityInternal = v1;
b1.AngularVelocityInternal = w1;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Transform xf1;
b1.GetTransform(out xf1);
// Compute the effective mass matrix.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = LocalAnchorB;
_u = r2 - b1.Sweep.c - r1;
// Handle singularity.
float length = _u.Length();
if (length > Settings.LinearSlop)
{
_u *= 1.0f / length;
}
else
{
_u = Vector2.Zero;
}
float cr1u = MathUtils.Cross(r1, _u);
float cr2u = MathUtils.Cross(r2, _u);
float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + 0 * cr2u * cr2u;
Debug.Assert(invMass > Settings.Epsilon);
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (Frequency > 0.0f)
{
float C = length - Length;
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
float d = 2.0f * _mass * DampingRatio * omega;
// Spring stiffness
float k = _mass * omega * omega;
// magic formulas
_gamma = step.dt * (d + step.dt * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * step.dt * k * _gamma;
_mass = invMass + _gamma;
_mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale the impulse to support a variable time step.
_impulse *= step.dtRatio;
Vector2 P = _impulse * _u;
b1.LinearVelocityInternal -= b1.InvMass * P;
b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
}
else
{
_impulse = 0.0f;
}
}
/// <summary>
/// Inits the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
internal override void InitVelocityConstraints(TimeStep step)
{
Body body1 = Body1;
Body body2 = Body2;
if (IsMotorEnabled || IsLimitEnabled)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
Box2DxDebug.Assert(body1.InvI > 0.0f || body2.InvI > 0.0f);
}
// Compute the effective mass matrix.
Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter());
Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2]
// [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2]
// [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2]
float m1 = body1.InvMass, m2 = body2.InvMass;
float i1 = body1.InvI, i2 = body2.InvI;
float col1X = m1 + m2 + mulR1.Y * mulR1.Y * i1 + mulR2.Y * mulR2.Y * i2;
float col2X = -mulR1.Y * mulR1.X * i1 - mulR2.Y * mulR2.X * i2;
float col3X = -mulR1.Y * i1 - mulR2.Y * i2;
float col1Y = Mass.Col2.X;
float col2Y = m1 + m2 + mulR1.X * mulR1.X * i1 + mulR2.X * mulR2.X * i2;
float col3Y = mulR1.X * i1 + mulR2.X * i2;
float col1Z = Mass.Col3.X;
float col2Z = Mass.Col3.Y;
float col3Z = i1 + i2;
Mass = new Mat33(new Vec3(col1X, col1Y, col1Z), new Vec3(col2X, col2Y, col2Z),
new Vec3(col3X, col3Y, col3Z));
/*
* _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
* _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
* _mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
* _mass.Col1.Y = _mass.Col2.X;
* _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
* _mass.Col3.Y = r1.X * i1 + r2.X * i2;
* _mass.Col1.Z = _mass.Col3.X;
* _mass.Col2.Z = _mass.Col3.Y;
* _mass.Col3.Z = i1 + i2;
*/
MotorMass = 1.0f / (i1 + i2);
if (IsMotorEnabled == false)
{
MotorTorque = 0.0f;
}
if (IsLimitEnabled)
{
float jointAngle = body2.Sweep.A - body1.Sweep.A - ReferenceAngle;
if (Box2DXMath.Abs(UpperLimit - LowerLimit) < 2.0f * Settings.AngularSlop)
{
State = LimitState.EqualLimits;
}
else if (jointAngle <= LowerLimit)
{
if (State != LimitState.AtLowerLimit)
{
Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f);
}
State = LimitState.AtLowerLimit;
}
else if (jointAngle >= UpperLimit)
{
if (State != LimitState.AtUpperLimit)
{
Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f);
}
State = LimitState.AtUpperLimit;
}
else
{
State = LimitState.InactiveLimit;
Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f);
}
}
else
{
State = LimitState.InactiveLimit;
}
if (step.WarmStarting)
{
// Scale impulses to support a variable time step.
Impulse *= step.DtRatio;
MotorTorque *= step.DtRatio;
Vec2 p = new Vec2(Impulse.X, Impulse.Y);
body1.LinearVelocity -= m1 * p;
body1.AngularVelocity -= i1 * (Vec2.Cross(mulR1, p) + MotorTorque + Impulse.Z);
body2.LinearVelocity += m2 * p;
body2.AngularVelocity += i2 * (Vec2.Cross(mulR2, p) + MotorTorque + Impulse.Z);
}
else
{
Impulse.SetZero();
MotorTorque = 0.0f;
}
}
protected internal abstract void RunLogic(TimeStep step);
/// <summary>
/// Solves the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = Body1;
Body b2 = Body2;
Vec2 v1 = b1.LinearVelocity;
float w1 = b1.AngularVelocity;
Vec2 v2 = b2.LinearVelocity;
float w2 = b2.AngularVelocity;
float m1 = b1.InvMass, m2 = b2.InvMass;
float i1 = b1.InvI, i2 = b2.InvI;
//Solve motor constraint.
if (IsMotorEnabled && State != LimitState.EqualLimits)
{
float cdot = w2 - w1 - MotorSpeed;
float impulse = MotorMass * -cdot;
float oldImpulse = MotorTorque;
float maxImpulse = step.Dt * MaxMotorTorque;
MotorTorque = Box2DXMath.Clamp(MotorTorque + impulse, -maxImpulse, maxImpulse);
impulse = MotorTorque - oldImpulse;
w1 -= i1 * impulse;
w2 += i2 * impulse;
}
//Solve limit constraint.
if (IsLimitEnabled && State != LimitState.InactiveLimit)
{
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, LocalAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, LocalAnchor2 - b2.GetLocalCenter());
// Solve point-to-point constraint
Vec2 cdot1 = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1);
float cdot2 = w2 - w1;
Vec3 cdot = new Vec3(cdot1.X, cdot1.Y, cdot2);
Vec3 impulse = Mass.Solve33(-cdot);
if (State == LimitState.EqualLimits)
{
Impulse += impulse;
}
else if (State == LimitState.AtLowerLimit)
{
float newImpulse = Impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
Vec2 reduced = Mass.Solve22(-cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -Impulse.Z;
Impulse = new Vec3(reduced.X, reduced.Y, 0.0f);
}
}
else if (State == LimitState.AtUpperLimit)
{
float newImpulse = Impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vec2 reduced = Mass.Solve22(-cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -Impulse.Z;
Impulse = new Vec3(reduced.X, reduced.Y, 0.0f);
}
}
Vec2 p = new Vec2(impulse.X, impulse.Y);
v1 -= m1 * p;
w1 -= i1 * (Vec2.Cross(r1, p) + impulse.Z);
v2 += m2 * p;
w2 += i2 * (Vec2.Cross(r2, p) + impulse.Z);
}
else
{
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, LocalAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, LocalAnchor2 - b2.GetLocalCenter());
// Solve point-to-point constraint
Vec2 cdot = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1);
Vec2 impulse = Mass.Solve22(-cdot);
Impulse = new Vec3(impulse.X, impulse.Y, Impulse.Z);
v1 -= m1 * impulse;
w1 -= i1 * Vec2.Cross(r1, impulse);
v2 += m2 * impulse;
w2 += i2 * Vec2.Cross(r2, impulse);
}
b1.LinearVelocity = v1;
b1.AngularVelocity = w1;
b2.LinearVelocity = v2;
b2.AngularVelocity = w2;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
Vector2 p1 = b1.Sweep.c + r1;
Vector2 p2 = b2.Sweep.c + r2;
Vector2 s1 = GroundAnchorA;
Vector2 s2 = GroundAnchorB;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.Length();
float length2 = _u2.Length();
if (length1 > Settings.LinearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.Zero;
}
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _ant - length1 - Ratio * length2;
if (C > 0.0f)
{
_state = LimitState.Inactive;
_impulse = 0.0f;
}
else
{
_state = LimitState.AtUpper;
}
if (length1 < _maxLengthA)
{
_limitState1 = LimitState.Inactive;
_limitImpulse1 = 0.0f;
}
else
{
_limitState1 = LimitState.AtUpper;
}
if (length2 < _maxLengthB)
{
_limitState2 = LimitState.Inactive;
_limitImpulse2 = 0.0f;
}
else
{
_limitState2 = LimitState.AtUpper;
}
// Compute effective mass.
float cr1u1 = MathUtils.Cross(r1, _u1);
float cr2u2 = MathUtils.Cross(r2, _u2);
_limitMass1 = b1.InvMass + b1.InvI * cr1u1 * cr1u1;
_limitMass2 = b2.InvMass + b2.InvI * cr2u2 * cr2u2;
_pulleyMass = _limitMass1 + Ratio * Ratio * _limitMass2;
Debug.Assert(_limitMass1 > Settings.Epsilon);
Debug.Assert(_limitMass2 > Settings.Epsilon);
Debug.Assert(_pulleyMass > Settings.Epsilon);
_limitMass1 = 1.0f / _limitMass1;
_limitMass2 = 1.0f / _limitMass2;
_pulleyMass = 1.0f / _pulleyMass;
if (Settings.EnableWarmstarting)
{
// Scale impulses to support variable time steps.
_impulse *= step.dtRatio;
_limitImpulse1 *= step.dtRatio;
_limitImpulse2 *= step.dtRatio;
// Warm starting.
Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
Vector2 P2 = (-Ratio * _impulse - _limitImpulse2) * _u2;
b1.LinearVelocityInternal += b1.InvMass * P1;
b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
b2.LinearVelocityInternal += b2.InvMass * P2;
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
}
else
{
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
}
public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
{
// Integrate velocities and apply damping.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.IsStatic())
{
continue;
}
// Integrate velocities.
b._linearVelocity += step.Dt * (gravity + b._invMass * b._force);
b._angularVelocity += step.Dt * b._invI * b._torque;
// Reset forces.
b._force.Set(0.0f, 0.0f);
b._torque = 0.0f;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
b._linearVelocity *= Common.MathB2.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f);
b._angularVelocity *= Common.MathB2.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f);
}
ContactSolver contactSolver = new ContactSolver(step, _contacts, _contactCount);
// Initialize velocity constraints.
contactSolver.InitVelocityConstraints(step);
for (int i = 0; i < _jointCount; ++i)
{
_joints[i].InitVelocityConstraints(step);
}
// Solve velocity constraints.
for (int i = 0; i < step.VelocityIterations; ++i)
{
for (int j = 0; j < _jointCount; ++j)
{
_joints[j].SolveVelocityConstraints(step);
}
contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
contactSolver.FinalizeVelocityConstraints();
// Integrate positions.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.IsStatic())
{
continue;
}
// Check for large velocities.
Vec2 translation = step.Dt * b._linearVelocity;
if (Common.Vec2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
translation.Normalize();
b._linearVelocity = (Settings.MaxTranslation * step.Inv_Dt) * translation;
}
float rotation = step.Dt * b._angularVelocity;
if (rotation * rotation > Settings.MaxRotationSquared)
{
if (rotation < 0.0)
{
b._angularVelocity = -step.Inv_Dt * Settings.MaxRotation;
}
else
{
b._angularVelocity = step.Inv_Dt * Settings.MaxRotation;
}
}
// Store positions for continuous collision.
b._sweep.C0 = b._sweep.C;
b._sweep.A0 = b._sweep.A;
// Integrate
b._sweep.C += step.Dt * b._linearVelocity;
b._sweep.A += step.Dt * b._angularVelocity;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int i = 0; i < step.PositionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool jointsOkay = true;
for (int j = 0; j < _jointCount; ++j)
{
bool jointOkay = _joints[j].SolvePositionConstraints(Settings.ContactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(contactSolver._constraints);
if (allowSleep)
{
float minSleepTime = Settings.FLT_MAX;
#if !TARGET_FLOAT32_IS_FIXED
float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
#endif
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b._invMass == 0.0f)
{
continue;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0 ||
#if TARGET_FLOAT32_IS_FIXED
Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance ||
Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
#else
b._angularVelocity *b._angularVelocity > angTolSqr ||
Vec2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
#endif
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b._sleepTime += step.Dt;
minSleepTime = Common.MathB2.Min(minSleepTime, b._sleepTime);
}
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b = BodyA;
float mass = b.Mass;
// Frequency
float omega = 2.0f*Settings.Pi*Frequency;
// Damping coefficient
float d = 2.0f*mass*DampingRatio*omega;
// Spring stiffness
float k = mass*(omega*omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
Debug.Assert(d + step.dt*k > Settings.Epsilon);
_gamma = step.dt*(d + step.dt*k);
if (_gamma != 0.0f)
{
_gamma = 1.0f/_gamma;
}
_beta = step.dt*k*_gamma;
// Compute the effective mass matrix.
Transform xf1;
b.GetTransform(out xf1);
Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b.LocalCenter);
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y]
// [ 0 1/m1+1/m2] [-r1.X*r1.Y r1.X*r1.X] [-r1.X*r1.Y r1.X*r1.X]
float invMass = b.InvMass;
float invI = b.InvI;
Mat22 K1 = new Mat22(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass));
Mat22 K2 = new Mat22(new Vector2(invI*r.Y*r.Y, -invI*r.X*r.Y),
new Vector2(-invI*r.X*r.Y, invI*r.X*r.X));
Mat22 K;
Mat22.Add(ref K1, ref K2, out K);
K.Col1.X += _gamma;
K.Col2.Y += _gamma;
_mass = K.Inverse;
_C = b.Sweep.C + r - _worldAnchor;
// Cheat with some damping
b.AngularVelocityInternal *= 0.98f;
// Warm starting.
_impulse *= step.dtRatio;
b.LinearVelocityInternal += invMass*_impulse;
b.AngularVelocityInternal += invI*MathUtils.Cross(r, _impulse);
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
Vector2 p1 = b1.Sweep.C + r1;
Vector2 p2 = b2.Sweep.C + r2;
Vector2 s1 = GroundAnchorA;
Vector2 s2 = GroundAnchorB;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.Length();
float length2 = _u2.Length();
if (length1 > Settings.LinearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.Zero;
}
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _ant - length1 - Ratio * length2;
if (C > 0.0f)
{
_state = LimitState.Inactive;
_impulse = 0.0f;
}
else
{
_state = LimitState.AtUpper;
}
if (length1 < _maxLengthA)
{
_limitState1 = LimitState.Inactive;
_limitImpulse1 = 0.0f;
}
else
{
_limitState1 = LimitState.AtUpper;
}
if (length2 < _maxLengthB)
{
_limitState2 = LimitState.Inactive;
_limitImpulse2 = 0.0f;
}
else
{
_limitState2 = LimitState.AtUpper;
}
// Compute effective mass.
float cr1u1 = MathUtils.Cross(r1, _u1);
float cr2u2 = MathUtils.Cross(r2, _u2);
_limitMass1 = b1.InvMass + b1.InvI * cr1u1 * cr1u1;
_limitMass2 = b2.InvMass + b2.InvI * cr2u2 * cr2u2;
_pulleyMass = _limitMass1 + Ratio * Ratio * _limitMass2;
Debug.Assert(_limitMass1 > Settings.Epsilon);
Debug.Assert(_limitMass2 > Settings.Epsilon);
Debug.Assert(_pulleyMass > Settings.Epsilon);
_limitMass1 = 1.0f / _limitMass1;
_limitMass2 = 1.0f / _limitMass2;
_pulleyMass = 1.0f / _pulleyMass;
if (Settings.EnableWarmstarting)
{
// Scale impulses to support variable time steps.
_impulse *= step.dtRatio;
_limitImpulse1 *= step.dtRatio;
_limitImpulse2 *= step.dtRatio;
// Warm starting.
Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
Vector2 P2 = (-Ratio * _impulse - _limitImpulse2) * _u2;
b1.LinearVelocityInternal += b1.InvMass * P1;
b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
b2.LinearVelocityInternal += b2.InvMass * P2;
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
}
else
{
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
}
protected internal override void RunLogic(TimeStep step)
{
for (int index = 0; index < items.Count; ++index)
{
Wrapper wrapper = items[index];
Body body = wrapper.body;
if (wrapper.affectable == null ||
body.IgnoresPhysicsLogics ||
Scalar.IsPositiveInfinity(body.Mass.Mass))
{
continue;
}
IShape shape = body.Shape;
int isInsideCount = 0;
foreach (Vector2D corner in body.Rectangle.Corners())
{
Scalar distance = line.GetDistance(corner);
if (distance <= 0)
{
isInsideCount++;
}
}
if (isInsideCount == 0) { continue; }
if (isInsideCount != 4)
{
Vector2D relativeVelocity = Vector2D.Zero;
Vector2D velocityDirection = Vector2D.Zero;
Vector2D dragDirection = Vector2D.Zero;
Vector2D centroid = Vector2D.Zero;
Line bodyLine;
Line.Transform(ref body.Matrices.ToBody, ref line, out bodyLine);
GetTangentCallback callback = delegate(Vector2D centTemp)
{
centroid = body.Matrices.ToWorldNormal * centTemp;
PhysicsHelper.GetRelativeVelocity(ref body.State.Velocity, ref centroid, out relativeVelocity);
relativeVelocity = FluidVelocity - relativeVelocity;
velocityDirection = relativeVelocity.Normalized;
dragDirection = body.Matrices.ToBodyNormal * velocityDirection.LeftHandNormal;
return dragDirection;
};
FluidInfo lineInfo = wrapper.affectable.GetFluidInfo(callback, bodyLine);
if (lineInfo == null) { continue; }
// Vector2D centTemp = lineInfo.Centroid;
Scalar areaTemp = lineInfo.Area;
// Vector2D centroid = body.Matrices.ToWorldNormal * centTemp;
Vector2D buoyancyForce = body.State.Acceleration.Linear * areaTemp * -Density;
body.ApplyForce(buoyancyForce, centroid);
/*
PhysicsHelper.GetRelativeVelocity(ref body.State.Velocity, ref centroid, out relativeVelocity);
relativeVelocity = FluidVelocity-relativeVelocity;
velocityDirection = relativeVelocity.Normalized;
dragDirection = body.Matrices.ToBodyNormal * velocityDirection.LeftHandNormal;
lineInfo = wrapper.affectable.GetFluidInfo(dragDirection, bodyLine);
if (lineInfo == null) { continue; }*/
Scalar speedSq = relativeVelocity.MagnitudeSq;
Scalar dragForceMag = -.5f * Density * speedSq * lineInfo.DragArea * DragCoefficient;
Scalar maxDrag = -MathHelper.Sqrt(speedSq) * body.Mass.Mass * step.DtInv;
if (dragForceMag < maxDrag)
{
dragForceMag = maxDrag;
}
Vector2D dragForce = dragForceMag * velocityDirection;
body.ApplyForce(dragForce, body.Matrices.ToWorldNormal * lineInfo.DragCenter);
body.ApplyTorque(
-body.Mass.MomentOfInertia *
(body.Coefficients.DynamicFriction + Density + DragCoefficient) *
body.State.Velocity.Angular);
}
else
{
Vector2D relativeVelocity = body.State.Velocity.Linear - FluidVelocity;
Vector2D velocityDirection = relativeVelocity.Normalized;
Vector2D dragDirection = body.Matrices.ToBodyNormal * velocityDirection.LeftHandNormal;
Vector2D centroid = wrapper.body.Matrices.ToWorldNormal * wrapper.affectable.Centroid;
Vector2D buoyancyForce = body.State.Acceleration.Linear * wrapper.affectable.Area * -Density;
wrapper.body.ApplyForce(buoyancyForce, centroid);
if (velocityDirection == Vector2D.Zero) { continue; }
DragInfo dragInfo = wrapper.affectable.GetFluidInfo(dragDirection);
if (dragInfo.DragArea < .01f) { continue; }
Scalar speedSq = relativeVelocity.MagnitudeSq;
Scalar dragForceMag = -.5f * Density * speedSq * dragInfo.DragArea * DragCoefficient;
Scalar maxDrag = -MathHelper.Sqrt(speedSq) * body.Mass.Mass * step.DtInv;
if (dragForceMag < maxDrag)
{
dragForceMag = maxDrag;
}
Vector2D dragForce = dragForceMag * velocityDirection;
wrapper.body.ApplyForce(dragForce, body.Matrices.ToWorldNormal * dragInfo.DragCenter);
wrapper.body.ApplyTorque(
-body.Mass.MomentOfInertia *
(body.Coefficients.DynamicFriction + Density + DragCoefficient) *
body.State.Velocity.Angular);
}
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
if (_state == LimitState.AtUpper)
{
Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
float Cdot = -Vector2.Dot(_u1, v1) - Ratio * Vector2.Dot(_u2, v2);
float impulse = _pulleyMass * (-Cdot);
float oldImpulse = _impulse;
_impulse = Math.Max(0.0f, _impulse + impulse);
impulse = _impulse - oldImpulse;
Vector2 P1 = -impulse * _u1;
Vector2 P2 = -Ratio * impulse * _u2;
b1.LinearVelocityInternal += b1.InvMass * P1;
b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
b2.LinearVelocityInternal += b2.InvMass * P2;
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
}
if (_limitState1 == LimitState.AtUpper)
{
Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
float Cdot = -Vector2.Dot(_u1, v1);
float impulse = -_limitMass1 * Cdot;
float oldImpulse = _limitImpulse1;
_limitImpulse1 = Math.Max(0.0f, _limitImpulse1 + impulse);
impulse = _limitImpulse1 - oldImpulse;
Vector2 P1 = -impulse * _u1;
b1.LinearVelocityInternal += b1.InvMass * P1;
b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
}
if (_limitState2 == LimitState.AtUpper)
{
Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
float Cdot = -Vector2.Dot(_u2, v2);
float impulse = -_limitMass2 * Cdot;
float oldImpulse = _limitImpulse2;
_limitImpulse2 = Math.Max(0.0f, _limitImpulse2 + impulse);
impulse = _limitImpulse2 - oldImpulse;
Vector2 P2 = -impulse * _u2;
b2.LinearVelocityInternal += b2.InvMass * P2;
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
}
}