public Car(ISpeedProvider moveSpeed, ISpeedProvider rotateSpeed, IAcceleration acceleration, ISurfaceInfo surfaceInfo)
{
m_MoveSpeed = moveSpeed;
m_RotateSpeed = rotateSpeed;
m_Acceleration = acceleration;
m_SurfaceInfo = surfaceInfo;
}
/// <summary>
///
/// </summary>
/// <param name="abbreviation"></param>
/// <param name="toBase"></param>
/// <param name="fromBase"></param>
/// <param name="mass"></param>
/// <param name="acceleration"></param>
protected ForceBase(string abbreviation,
IUnitConversion toBase, IUnitConversion fromBase,
IMass mass, IAcceleration acceleration)
: base(abbreviation, toBase, fromBase, mass, acceleration)
{
VerifyDimensions();
}
/// <summary>
///
/// </summary>
/// <param name="abbreviation"></param>
/// <param name="toBase"></param>
/// <param name="fromBase"></param>
/// <param name="mass"></param>
/// <param name="acceleration"></param>
protected ForceBase(string abbreviation,
IUnitConversion toBase, IUnitConversion fromBase,
IMass mass, IAcceleration acceleration)
: base(abbreviation, toBase, fromBase, mass, acceleration)
{
VerifyDimensions();
}
public void Init(IGame game, ISpeedProvider moveSpeed, ISpeedProvider rotateSpeed, IAcceleration acceleration, ISurfaceInfo surfaceInfo)
{
m_Game = game;
m_MoveSpeed = moveSpeed;
m_RotateSpeed = rotateSpeed;
m_Acceleration = acceleration;
m_SurfaceInfo = surfaceInfo;
}
private void PostCreateFrame()
{
try
{
frame = this.GetFrame();
acc = frame.GetSimpleObject <IAcceleration>("Object should have acceleration");
angvel = frame.GetSimpleObject <IAngularVelocity>("Object should have angular velocity");
vel = frame.GetSimpleObject <IVelocity>("Object should have velocity");
angacc = frame.GetSimpleObject <IAngularAcceleration>("Object should have angular acceleration");
}
catch (Exception e)
{
e.ShowError(10);
IPosition p = this;
p.Parent = null;
throw e;
}
}
/// <summary>
/// Sets state
/// </summary>
/// <param name="baseFrame">Base frame</param>
/// <param name="relative">Relative frame</param>
public override void Set(ReferenceFrame baseFrame, ReferenceFrame relative)
{
base.Set(baseFrame, relative);
IAcceleration ab = baseFrame as IAcceleration;
IAcceleration ar = relative as IAcceleration;
IAngularAcceleration arn = relative as IAngularAcceleration;
IVelocity vb = baseFrame as IVelocity;
IVelocity vr = relative as IVelocity;
IAngularVelocity anb = baseFrame as IAngularVelocity;
IAngularVelocity anr = relative as IAngularVelocity;
double[] rp = Position;
double[,] m = Matrix;
double[] omr = anr.Omega;
StaticExtensionVector3D.VectorPoduct(omr, vr.Velocity, tempV);
double om2 = StaticExtensionVector3D.Square(omr);
double[] eps = arn.AngularAcceleration;
StaticExtensionVector3D.VectorPoduct(eps, rp, temp);
for (int i = 0; i < 3; i++)
{
tempV[i] *= 2;
tempV[i] += om2 * rp[i] + relativeAcceleration[i] + temp[i];
}
RealMatrix.Multiply(m, tempV, acceleration);
double[] omb = anb.Omega;
IOrientation orr = relative as IOrientation;
double[,] mrr = orr.Matrix;
RealMatrix.Multiply(omb, mrr, temp);
StaticExtensionVector3D.VectorPoduct(temp, omr, tempV);
for (int i = 0; i < 3; i++)
{
temp[i] = eps[i] + tempV[i];
}
RealMatrix.Multiply(temp, m, angularAcceleration);
}
/// <summary>
/// Private Constructor
/// </summary>
/// <param name="abbreviation"></param>
/// <param name="toBase"></param>
/// <param name="fromBase"></param>
/// <param name="mass"></param>
/// <param name="acceleration"></param>
private Force(string abbreviation,
IUnitConversion toBase, IUnitConversion fromBase,
IMass mass, IAcceleration acceleration)
: base(abbreviation, toBase, fromBase, mass, acceleration)
{
}
public Car WithWheelAcceleration(IAcceleration acceleration)
{
return(new Car(_name, _wheelDrive, acceleration));
}
private Car(string name, IWheelDrive wheelDrive, IAcceleration acceleration)
{
_name = name;
_wheelDrive = wheelDrive;
_acceleration = acceleration;
}
private Force(string abbreviation,
IUnitConversion toBase, IUnitConversion fromBase,
IMass mass, IAcceleration acceleration)
: base(abbreviation, toBase, fromBase, mass, acceleration)
{
}
/**
* Set the delegate implementation.
*
* @param _delegate The delegate implementing platform specific functions.
*/
public void SetDelegate(IAcceleration _delegate)
{
this._delegate = _delegate;
}
/**
* Constructor with delegate.
*
* @param _delegate The delegate implementing platform specific functions.
*/
public AccelerationBridge(IAcceleration _delegate) : base()
{
this._delegate = _delegate;
}
public AccelerationMove(Transform transform, ISpeed speed, IAcceleration acceleration)
: base(transform, speed)
{
_acceleration = acceleration.Acceleration;
}
void IDifferentialEquationSolver.CalculateDerivations()
{
Normalize(aggregate);
foreach (AggregableWrapper aw in aggrWrappres)
{
IAggregableMechanicalObject obj = aw.Aggregate;
if (obj is Diagram.UI.IUpdatableObject)
{
Diagram.UI.IUpdatableObject uo = obj as Diagram.UI.IUpdatableObject;
if (uo.Update != null)
{
uo.Update();
}
}
}
Solve();
int n = 0;
int kv = 0;
for (int i = 0; i < aggrWrappres.Length; i++)
{
AggregableWrapper wrapper = aggrWrappres[i];
IAggregableMechanicalObject agg = wrapper.Aggregate;
Motion6DAcceleratedFrame frame = wrapper.OwnFrame;
IVelocity vel = frame;
IAcceleration acc = frame;
IPosition pos = frame;
double[] state = agg.State;
double[] p = pos.Position;
double[] v = vel.Velocity;
for (int j = 0; j < 3; j++)
{
p[j] = state[j];
derivations[n + j, 0] = state[j];
double a = state[j + 3];
v[j] = a;
derivations[n + j, 1] = a;
derivations[n + 3 + j, 0] = a;
derivations[n + 3 + j, 1] = vector[kv];
++kv;
}
IOrientation or = frame;
double[] q = or.Quaternion;
for (int j = 0; j < 4; j++)
{
double a = state[j + 6];
quater[j] = a;
q[j] = a;
}
IAngularVelocity av = frame;
double[] om = av.Omega;
for (int j = 0; j < 3; j++)
{
double a = state[j + 10];
omega[j] = a;
om[j] = a;
}
StaticExtensionVector3D.CalculateQuaternionDerivation(quater, omega, der, auxQuaternion);
for (int j = 0; j < 4; j++)
{
derivations[n + 6 + j, 0] = quater[j];
derivations[n + 6 + j, 1] = der[j];
}
for (int j = 0; j < 3; j++)
{
derivations[n + 10 + j, 0] = omega[j];
derivations[n + 10 + j, 1] = vector[kv];
++kv;
}
int kk = n + 13;
int stk = kk;
int stv = 6;
int sk = 13;
for (int j = 13; j < agg.Dimension; j++)
{
derivations[kk, 0] = state[sk];
double a = state[sk + 1];
derivations[kk, 1] = a;
++kk;
++stk;
++sk;
derivations[kk, 0] = a;
derivations[kk, 1] = vector[kv];
++sk;
++kv;
++stv;
++kk;
++j;
}
n += agg.Dimension;
}
}
/// <summary>
/// Updates itself
/// </summary>
public override void Update()
{
try
{
this.FullReset();
IDataConsumer cons = this;
cons.UpdateChildrenData();
ReferenceFrame relative = Relative;
IPosition p = relative;
double[] x = p.Position;
ReferenceFrame parent = this.GetParentFrame();
for (int i = 0; i < 3; i++)
{
x[i] = (double)measurements[i].Parameter();
}
if (relative is IVelocity)
{
IVelocity vel = relative as IVelocity;
double[] v = vel.Velocity;
for (int i = 0; i < 3; i++)
{
IDerivation d = measurements[i] as IDerivation;
v[i] = Measurement.GetDouble(d.Derivation);
}
}
double[] qua = relative.Quaternion;
for (int i = 0; i < qua.Length; i++)
{
IMeasurement m = measurements[i + 3];
qua[i] = (double)m.Parameter();
}
relative.SetMatrix();
if (relative is IAngularVelocity)
{
IAngularVelocity av = relative as IAngularVelocity;
double[] om = av.Omega;
IOrientation or = relative as IOrientation;
for (int i = 0; i < 4; i++)
{
IDerivation d = measurements[i + 3] as IDerivation;
der[i] = Measurement.GetDouble(d.Derivation);
}
Vector3D.StaticExtensionVector3D.CalculateDynamics(or.Quaternion, der, om, qd);
}
if (relative is IAcceleration)
{
IAcceleration acc = relative as IAcceleration;
IAngularAcceleration anc = relative as IAngularAcceleration;
double[] linacc = acc.RelativeAcceleration;
for (int i = 0; i < linacc.Length; i++)
{
linacc[i] = Measurement.GetDouble(secondDeriM[i]);
}
for (int i = 0; i < 4; i++)
{
angsec[i] = Measurement.GetDouble(secondDeriM[i + 3]);
}
IAngularVelocity av = relative as IAngularVelocity;
double[] om = av.Omega;
IOrientation or = relative as IOrientation;
double[] angacc = anc.AngularAcceleration;
Vector3D.StaticExtensionVector3D.CalculateAcceleratedDynamics(or.Quaternion, der, om, qd, angsec, angacc);
}
base.Update();
}
catch (Exception exception)
{
exception.ShowError(10);
//!!! OLD this.Throw(ex);
}
}
