public void Run(TimeSpan dt)
{
double seconds = dt.TotalSeconds;
// Stand at a safe distance, we are about to do physics
Vector2D totalForce = new Vector2D(); //Newtons, in x,y
double totalTorque = 0; // N*m
// Calculate x, y forces and torque
foreach (var forceMoment in forces.Values)
{
var force = forceMoment.Item1;
var momentArm = forceMoment.Item2;
totalForce.x += force.x;
totalForce.y += force.y;
// calculate length of moment arm in meters
double r = momentArm.Magnitude;
// determine angle between force and moment arm (rad)
//double theta = Util.ToPiNegPi(force.AngleRad - momentArm.AngleRad);
double theta = force.AngleRad - momentArm.AngleRad;
// torque = the length of moment arm * force parallel to moment arm
double torque = force.Magnitude * r * Math.Sin(theta);
totalTorque += torque;
}
// calc X, Y acceleration
acceleration.x = totalForce.x / MassProp.Mass;
acceleration.y = totalForce.y / MassProp.Mass;
// calc angular acceleration
// todo: deal with zero MoI
angularAcceleration = totalTorque / MassProp.MomentOfInertia;
// add gravity
if (Gravity != null)
{
var gravAccel = Gravity.GetAcceleration(position);
acceleration.x += gravAccel.x;
acceleration.y += gravAccel.y;
}
// Update position
position.x += velocity.x * seconds;
position.y += velocity.y * seconds;
velocity.x += acceleration.x * seconds;
velocity.y += acceleration.y * seconds;
// Update orientation
orientation = Util.To180(orientation + angularVelocity * seconds);
angularVelocity += angularAcceleration * seconds;
}
void Start()
{
gravity = new Gravity (Application.dataPath+"/Resources/Dg01_cnt2.5x2.bytes");
Input.location.Start(); //enable location settings
Input.compass.enabled = true;
locationStatus = LocationServiceStatus.Running;
lon = 0; alt = 0; lat = 0;
dlon = 0; dalt = 0; dlat = 0;
gAn=gravity.GetGravity(lat,lon,alt);
gAcc = gravity.GetAcceleration (gAn);
startLocationService(); //begin GPS transmittion
}