## 1. Introduction

Every real science and engineering system is a result integration of ingredients which belong to different branches of science. For example, the
GPS Bulldozer System requires integration of mechanics, hydraulics, and navigation.
The above picture represents the following ingredients of the problem:

- Bulldozer (main object)
- Navigation satellite (right top object)
- Inertial navigation system (left bottom object)
- Kalman filter (transparent formula at center)

A lot of difficulties are caused by that different problems have different software. This article shows that a universal software makes the integration process easier.
The main sample of this article is a GPS Bulldozer System.

## 2. Background

Here the relevant theoretical issues are considered.

### 2.1 Outlook

GPS Bulldozer Systems could have different configurations. The following references contain descriptions or some configurations:

Although these systems are different, they have very similar ingredients or building blocks:

- GPS antennas
- Gyro and accelerometers
- Principles of math processing

It is clear that if we have a framework which can configure these blocks, then the design of such a system would become easier. The main problem is automatic control
of the bulldozer blade. The classical automatic control scheme is represented in the following picture:

However, this problem requires a more complicated scheme which is presented below:

The interesting part of this scheme is the usage of indirect measurements (see also
http://wiki.answers.com/Q/What_is_indirect_measuring). The control system uses GPS measurements
of position and velocity of the bulldozer in an orthogonal reference frame. Also, measurements of Gyro and accelerometers are used.
But these measurements do not provide direct information about the real and required positions of the bulldozer blade. It is a complicated dependence between system measurements
and control parameters. So GPS and other measurements should be processed. The typical algorithm for this processing is the
Kalman Filter. The following section is devoted to it.

### 2.2 Kalman filter

#### 2.2.1 Theory

The Kalman filter is a mathematical method named after
Rudolf E. Kalman. Its purpose is to use measurements that are observed over
time that contain noise (random variations) and other inaccuracies, and produce values that tend to be closer
to the true values of the measurements and their associated calculated values. Kalman filter theory does not depend
on physical phenomenon. So Kalman filter can be used for the estimation of state of different types of objects (mechanical, electrical, pneumatic,
etc). Moreover, the Kalman filter theory does not depend on the types of measurements. Any type of measurement is acceptable (inertial, electrical, temperature, optical,
nuclear). So Kalman Filter is an engineering object which is invariant to the physical background. We have a universal engineering object and this idea is very close to
the idea of a universal engineering software. How can the Kalman Filter theory exist without any physical background? It is based on abstract equations. Some of these
equations are presented below:

where *x*_{k- 1}, *x*_{k} are state vectors at *k* - 1 and *k*^{th} steps, respectively. *z*_{k}
is a vector of measurements. The vector functions *f* and *h* are respectively called transition function and function of measurements. All the above notions are abstract.
These notions are not directly related to any special engineering problem. In a GPS Bulldozer System, these parameters have the following meaning. The control operations are being performed by
a digital computer. So these operations are separated by steps. Vectors *x*_{k- 1}, *x*_{k} correspond to *k* - 1 and *k*^{th} steps.
If we consider a GPS Bulldozer System, then the components of vectors *x*_{k- 1}, *x*_{k} include the following parameters of motion
of the GPS Bulldozer System:

- Three coordinates of the bulldozer;
- The components of the bulldozer 3D velocity;
- Quaternion of the bulldozer orientation;
- Three components of the angular velocity of the bulldozer;
- Position of the blade lifting cylinder;
- Position of the blade tilting cylinder;
- Speed of movement of lifting the hydraulic cylinder;
- Speed of movement of tilting the hydraulic cylinder.

Detailed information about these parameters is contained
here. Vector *z*_{k} depends on the sensor which is used in the system. The *z*_{k} vector contains the GPS measurements. The number of components
of *z*_{k} depends on the number of GPS antennas. The system can contain accelerometers and gyro. In this case, these measurements are also contained in
*z*_{k}. Let us explain the meaning of *f* and *h*. First of all, note that the behavior of the system is described by
ordinary differential equations. These equations are known. The standard procedure
of integration of ordinary differential equations provides the function *f*. Similarly, well known formulas of geometry and kinematics provide the expression of *h*.

#### 2.2.2 Discussion

The previous section contains a lot of notions. The engineer should understand them. However, the engineer should not be overloaded by these notions in everyday work. A lot
of operations related to the problems should be automated. For example, obtaining *f* from differential equations is a standard procedure and it should be
automated. The derivation of equations also should be automated. In the best situation, the engineer sets the configuration of the bulldozer and (virtually) installs sensors on it.
After this action, the math model should be automatically generated. Then the engineer attaches the Kalman Filter to the generated model.

#### 2.2.3 Implementation

##### 2.2.3.1 Ingredients

The Kalman Filter theory deals with abstract vector functions *f* and *h*. The `IObjectTransformer`

interface is used for these function implementations:

public interface IObjectTransformer
{
string[] Input { get; }
string[] Output { get; }
object GetInputType(int i);
object GetOutputType(int i);
void Calculate(object[] input, object[] output);
}

This interface transforms a set of inputs to a set of outputs. It is not used for Kalman Filter only and in general, a number of inputs can exceed 1 as well as outputs.
However, this implementation of the Kalman Filter accepts a single input and a single output. An acceptable situation is presented in the following table:

N | Vector function | Number of inputs | Number of outputs | Input type | Output type |

1 | *f* | 1 | 1 | `double[n]` | `double[n]` |

2 | *h* | 1 | 1 | `double[n]` | `double[l]` |

Where *n* and *l* are dimensions of *x*_{k} and *z*_{k}, respectively. The implementation of Kalman Filter is contained
in the source code (class `KalmanFilter`

) and readers with a strong interest in the problem could study it. Here is a brief explanation of a simple application of it.
In 2.2.1, I noted that the *f* function could be obtained by the solution of ordinary differential equations. The following situation represents
an implementation of this idea by the framework:

Here the **Diff equations** object represents the following ordinary differential equations system. It has the following properties:

It means that this object represents the following system of differential equations:

where *x*, *y*, *z* are variables and *a*, *b*, *c*, *d*, *f*, *g*, *h*, *i*, *j*
are constants. The `State`

object has the
icon. This icon is a combination of the following icons: and
. The first one is a collection icon,
and the second one is a transformer icon. So the **State** object is a collection and transformer at the same time. There are two programming
ways to be a collection and transformer at the same time. The first way is multiple inheritance which has been considered in my previous article.
The second way is the usage of two objects, one of them a collection and the second one a transformer. Using this way, we can use several type
of transformers for one collection. This fact could reduce the amount of code. The **State** object is an object of the `DataPerformerCollectionStateTransformer`

type:

public class DataPerformerCollectionStateTransformer : ObjectsCollection,
IChildrenObject, IPostSetArrow, IRemovableObject

This class is a subclass of `ObjectsCollection`

. This fact means that `DataPerformerCollectionStateTransformer`

is a collection of objects. Also,
`DataPerformerCollectionStateTransformer`

implements the `IChildrenObject`

interface. One of the children is a transformer. The following code snippet shows this fact:

AbstractDoubleTransformer transformer;
void SetChildren()
{
if (transformer != null)
{
if (transformer.GetType().Equals(transformerType))
{
return;
}
IDisposable d = transformer;
d.Dispose();
}
if (transformerType.Equals(typeof(StateTransformer)))
{
transformer = new StateTransformer(this);
}
else
{
transformer = new StateVariableTransformer(this);
}
childen[0] = transformer;
}
#region IChildrenObject Members
IAssociatedObject[] IChildrenObject.Children
{
get { return childen; }
}
#endregion

The type of child is `AbstractDoubleTransformer`

. This type implements the `IObjectTransformer`

interface. So it is a transformer.
The class `AbstractDoubleTransformer`

is abstract. One of its subtypes is really used. The usage of multiple inheritance does not provide this facility.
There is a natural question: how can a child be found. The following snippet represents the function for this purpose:

public static T GetObject<T>(IAssociatedObject obj) where T : class
{
if (obj is T)
{
return obj as T;
}
if (obj is IChildrenObject)
{
IChildrenObject co = obj as IChildrenObject;
IAssociatedObject[] ch = co.Children;
if (ch != null)
{
foreach (IAssociatedObject ob in ch)
{
T a = GetObject<T>(ob);
if (a != null)
{
return a;
}
}
}
}
return null;
}

The following code can be used for finding an object which implements `IObjectTransformer`

:

IObjectTransformer transformer = this.GetObject<IObjectTransformer>(obj));

So the **State** object is a collection and transformer at the same time. As a collection, it could be the start of a "belongs arrow"
. This arrow connects the collection with its elements.
It means that **Diff equations** is a child of the **State** object. But **State** is also a transformer
(it implements the `IObjectTransformer`

interface). Any such object could be an end of arrow which connects the transformer and the consumer of the transformer.
This situation is presented below:

The
links **State** as `IObjectTransformer`

with the **Consumer** object as `IObjectTransformerConsumer`

.
The properties of the **State** object are presented below:

Let us explain these properties. First of all, the bottom combo box shows the type of the `transformer`

field type. Remember that
the transformer is an object of the abstract class `AbstractDoubleTransformer`

. The real transformer object could be
an object of a subclass of `AbstractDoubleTransformer`

. There are two subclasses of `AbstractDoubleTransformer`

:

`StateTransformer`

;
`StateVariableTransformer.`

.

The "State - State" text in the combo box means that the type of the `transformer`

field is `StateTransformer`

.
An object of `StateTransformer`

implements `IObjectTransformer`

by the following way:

public override void Calculate(object[] input, object[] output)
{
ITimeMeasureProvider old = processor.TimeProvider;
try
{
using (new TimeProviderBackup(collection, provider,
DifferentialEquationProcessor.Processor))
{
using (new ComponentCollectionBackup(collection))
{
processor.TimeProvider = provider;
double[] inp = input[0] as double[];
collection.SetStateVector(inp);
double start = provider.Time;
double step = provider.Step;
runtime.StartAll(start);
runtime.UpdateAll();
processor.Step(start, start + step);
provider.Time = start + step;
collection.GetStateVector(outbuffer);
output[0] = outbuffer;
}
}
}
catch (Exception ex)
{
ex.Log();
}
processor.TimeProvider = old;
}

This code snippet has the following informal explanation. The object of the `StateTransformer`

class collects all systems of ordinary
differential equations of a collection of objects. In result, we have a common system of differential equations. The `Calculate`

function performs
integration of differential equations and transforms the initial state vector of the equations to the final one as described in 2.2.1. The final time is separated
from the initial time by a Step. According to the above properties, the value of Step is equal to 0.1. The following situation presents a case where the type of `transformer`

is `StateVariableTransformer`

:

Here, the properties of the **Diff equations** object are the same as above. We also have the **Data** object with the following properties:

These properties have the following transparent sense. The output of the **Data** object is calculated by the following expressions:

*Formula_1* = *ax + by + cz*;

*Formula_2* = *dx + fy + gz*.

where *x*, *y*, *z* are state variables of the **Diff equations** object and *a*, *b*, *c*, *d*,
*f*, *g* are constants. The **Vector** object has the following properties:

It means that the output of the `Vector`

object is a 2D vector. The components of the vector are *Formula_1* and *Formula_2* of the **Data**
object. The properties of the **Mea** object are presented below:

These properties have the following meaning. The "State - Output" text means that the type of `transformer`

is `StateVariableTransformer`

. The Measurements
field of this form is "Vector". It means that the output of the **Mea** object is the output of the **Vector**. Roughly speaking, the **Mea**
object is a transformation from the state vector of **Diff equations** to **Vector**.

##### 2.2.3.2 Construction of Kalman Filter

Now we have the ingredients which enable us to construct the Kalman Filter. Let us construct it. First of all, let us provide the math formulation of the problem. Suppose that a dynamical
system with measurements is described by the following equations:

*x* is the state vector of system, *f* is the right part function of the differential equation system, *z* is the measurements vector, and *h* is
the function which links the measurements with the state vector. The system (1) is abstract since we do not know explicit expressions for *f* and *h*. Suppose that *f*
and *h* are defined by the following expressions:

Then the following scenario contains the Kalman Filter for such a model:

The following are the objects which were already considered in 2.2.3.2:

**Diff equations**
**Data**
**Vector**
**State**
**Mea**

The following mapping links the variables of equations (3) and the objects of this picture:

N | Variable of equations (3) | Object | Variable of object |

1 | *x*_{1} | **Diff equations** | *x* |

2 | *x*_{1} | **Diff equations** | *y* |

3 | *x*_{1} | **Diff equations** | *z* |

4 | *a*_{11} | **Diff equations** | *a* |

5 | *a*_{12} | **Diff equations** | *b* |

6 | *a*_{13} | **Diff equations** | *c* |

7 | *a*_{21} | **Diff equations** | *d* |

8 | *a*_{22} | **Diff equations** | *f* |

9 | *a*_{23} | **Diff equations** | *g* |

10 | *a*_{31} | **Diff equations** | *h* |

11 | *a*_{32} | **Diff equations** | *i* |

12 | *a*_{11} | **Diff equations** | *a* |

13 | *z*_{1} | **Data** | *Formula_1* |

14 | *z*_{2} | **Data** | *Formula_2* |

15 | *b*_{11} | **Data** | *a* |

16 | *b*_{12} | **Data** | *b* |

17 | *b*_{13} | **Data** | *c* |

18 | *b*_{21} | **Data** | *d* |

19 | *b*_{22} | **Data** | *f* |

20 | *b*_{23} | **Data** | *g* |

The objects **Measurements**, **State Covariation**, and **Measurements Covariation** are used for
the simulation of measurements, covarition of the system noise, and covariation of the measurements noise. An explanation of these parameters can be found
here. The **Filter** object represents the Kalman Filter and has the following properties:

The *State transformation* property corresponds to the vector function *f* in equation (1). The combo box of this property shows objects which implement
the `IObjectTransformer`

interface. The value of this property is *State*. So the **State** object as `IObjectTransformer`

corresponds
to *f*. Similarly, the **Mea** object as `IObjectTransformer`

corresponds to *Measurement transformation* or *h* in (1). Equations
(1) are presented below once again:

Other combo boxes contain the names of objects which implement the `IMeasurements`

interface. Their meanings
are explained above. Besides these parameters, the Kalman Filter uses matrixes of partial derivations:

There are two main paths of calculation of these matrixes. The first one based on math manipulations. The second one is the usage of finite difference. The first method is more
accurate but requires a lot of mental work. The second one is very simple but is less accurate. However, the author of this article does not yet know a problem which strongly
requires the first method. So this version of Kalman Filter uses finite differences and does not require special math expressions for these matrixes. It requires values
of finite differences which are represented in the above properties as *State difference* and *Measurement difference*, respectively.

### 2.3 Kinematics

The above example is a new form of representation of math expressions only. So it is not useful without integration with other branches of science and engineering.
Other branches of engineering also use math. But link to math could be implicit or declarative. Declarative programming has already proved its usefulness. Let us compare
declarative and imperative approaches in kinematics. The following picture contains an image of a typical kinematics problem.

We have three reference frames. We know the law of absolute 6D motion of frame **1**. Also, we know the law of relative 6D motion of frame **2**
with respect to frame **1** and relative motion of frame **3** with respect to frame **2** as well. Absolute motion of frame
**3** can be very complicated, even former **3** motion laws are simple. Suppose that we also have an additional **Base** frame
and we would like to define the motion law of frame **3** with respect to **Base**. The reflection of this situation is presented below:

The objects **1**, **2**, and **3** represent moved frames. The **L 1-2** arrow means that we consider relative motion
of frame **2** with respect to **1**. The **L 3-2** arrow has a similar meaning. However, the **1** frame is not
the beginning of any geometrical link . This fact means that
an absolute motion of the frame is considered. Frame **1** has the following properties:

Here, *X*, *Y*, *Z* are orthogonal coordinates of the frame, and *Q0*, *Q1*, *Q2*, *Q3* are components of quaternion
which define the orientation of the frame. The above properties contain a mapping between the 6D motion parameters of the frame and the parameters of the **Trans 1** object.
Properties of the *Trans 1* object are presented below:

The parameter *t* in the above formulas is time, other parameters are constants. The above formulae are not too complicated. The frames **2** and
**3** are similarly linked to **Trans 2** and **Trans 3**. However, coordinates and orientations of these frames are relative.
Although motion law of frame **1** and relative motion laws of frames **2** and **3** are very simple, absolute motion of frame
**3** is very complicated. However, the usage of the framework provides an easy definition of these law parameters. A combination of 6D motions can be obtained by
a connection by the geometrical link only. Suppose that we would like
to define parameters of motion of frame **3** with respect to frame **Base**. We have the following construction for this purpose:

The **Relative** object has the `RelativeMeasurements`

type. This object is connected by **M3** and **MB** arrows to
frames **3** and **Base**, respectively. All these facts mean that the **Relative** object provides parameters of relative motion
of frame **3** with respect to **Base**. The number of parameters depends on differentiability
of the relative 6D motion parameters. If these parameters are not differentiable
then the parameters include relative coordinates and orientation. If parameters are once
differentiable by time, then the parameters also include relative linear and angular velocities. A sufficient condition of
differentiability of frame **3** is differentiability of parameters of objects
**Trans 1**, **Trans 2**, and **Trans 3**. The following picture presents some properties of **Trans 1**:

This picture means that output parameters of the **Trans 1** object are once differentiable. In particular, the **Diff** object performs
time differentiation of parameters of **Trans 1** in the following way:

Since parameters of **Trans 1**, **Trans 2**, and **Trans3** are more than once differentiable, the **Relative** object
provides the following parameters:

If parameters of

**Trans 1** would not set at least once differentiable, then

**Relative** would provide relative coordinates and relative distance only:

The following picture represents the indication of relative motion parameters.

So the kinematics branch of the framework makes easy determination of relative motion parameters in very complicated situations. Moreover, it automatically determines
differentiability. This feature will be very essential for the discussion of mechanics issues. Some mechanical
trajectories should be twice differentiable.

### 2.4 Usage of Kinematics for 3D Graphics

A lot of software products have been designed strictly for specific problems. These products could not be supported and/or extended. For example, a lot of
CAD systems have been designed without perspective of integration with
CAE (see http://www.nafems.org/downloads/AUTOSIM_Meetings/ Barcelona_Spain_January_2006/autosim_barcelona06_schweiger.pdf).
The framework is a very farsighted project. It is clear that kinematics engineering software should be used for 3D animation. Otherwise, kinematics could exist without
animation. So there are a lot of compatible versions of the framework. I would like show the low coupling
of the framework. My articles are also devoted to the theme of foresighted projects. The animation was developed in 2002. OpenGL has been used for 3D Graphics
(see UniversalEnggFrmwork7.aspx). However, I knew in 2002 that more advanced technologies of 3D graphics will appear.
So 3D graphics has been developed with an abstract layer. And this layer is compatible with kinematics. This abstract layer is implemented as the following abstract class:

public abstract class PositionObjectFactory
{
static private PositionObjectFactory factory;
public static PositionObjectFactory Factory
{
get
{
return factory;
}
set
{
factory = value;
}
}
public abstract object CreateObject(string type);
public abstract Camera NewCamera();
public abstract object CreateForm(Camera camera);
public abstract object CreateForm(IPosition position, IVisible visible);
public abstract Type CameraType
{
get;
}
public abstract IObjectLabel CreateLabel(object obj);
public abstract object CreateLabel(IPosition position, IVisible visible);
}

This factory creates 3D objects, cameras, and elements of the UI. The abstract factory doed not know either OpenGL or WPF. It does not know even `System.Windows.Forms`

.
So the `object CreateForm`

method should not return `System.Windows.Forms.Form`

only. It returns a Form in a wide sense (for example, `System.Windows.Window`

).
There are two subclasses of `PositionObjectFactory`

:

`OpenGLFactory`

;
`WpfFactory`

.

Let us consider a sample of a 3D animation. In this sample, we have two airplanes. The following picture contains projections of the airplanes on two orthogonal planes:

Flight altitudes of the airplanes are different and so we do not have a collision. Since the kinematic module operates with relative positions, we can install a virtual camera
on board the planes. The following picture represents the views of these cameras:

The view of the camera installed on the first plane contains the image of the second plane, and vice versa. The following scheme represents the objects and links of this sample:

Let us explain the meaning of this picture. **Base 1** and **Base 2** are base frames of motion of the first and second airplanes, respectively.
The following picture reflects the orientations of the cameras:

This orientation reflects the fact that airplanes have almost opposite courses. The `Parameters`

object contains parameters of linear uniform motion.
This object has the following properties:

The **Motion 1** object is moving reference frame. Its motion parameters with respect to **Base 1** are represented below:

The essential part of this properties is *Formula_3* of **Parameters**. This formula is a linear dependence of time. *Formula_1* is equal to zero and
*Formula_2* is equal to unity. The **Bomber** object is a 3D model of the bomber. It is rigidly linked to the **Motion 1** reference frame.
So the motion of **Bomber** is the motion of the **Motion 1** reference frame. The **C 1** reference frame is rigidly linked to **Motion
1** and **Camera 1** is linked to **C 1**. So parameters of **C 1** are parameters of the relative position and orientation of
**Camera 1** with respect to **Motion 1** or **Bomber**. Otherwise, **Camera 1** is linked to **Transport**
by visibility link . So the viewport of **Camera 1** contains
the image of **Transport**. We have also **Motion 2**, **C 2**, and **Camera 2**.
The purpose of these objects is similar to **Motion 1**, **C 1**, and **Camera 1**.

To use this sample, set the following animation parameters:

and press the green arrow button.

### 2.3 Lagrange Mechanics

#### 2.3.1 Outlook

Bulldozer is an aggregate which is assembled from parts (see picture below):

The parts of the aggregate cannot be moved independently. The motion of parts are constrained. Lagrange mechanics provides a good model for constrained mechanical objects. The following
sections are devoted to the theory and software implementation of Lagrange mechanics.

#### 2.3.2 Theory

Lagrange mechanics is described by the Lagrange function
which depends on generalized coordinates *q*_{i} and their time derivations.
The Lagrange function is the difference between the kinetic energy and the potential one. Lagrange
equations are presented below:

Let us consider an example of a 2D pendulum. This example which is presented below:

The parameters
are generalized coordinates. Kinetic and potential energy (*T* and *U*) of the pendulum are presented below:

The pendulum is the building block of different objects of Lagrange mechanics. The complicated objects of Lagrange mechanics can be constructed from simple ones. Let us consider
a constrained system which contains two pendulums.

This system is constrained. Both coordinates *x*_{1}, *y*_{1} of the first pendulum are equal to zero. All constrains are presented by the following equations:

Generalized coordinates of two unconstrained pendulums are . Generalized coordinates of the constrained system of two pendulums are
. Lagrangian of the system can
be obtained by the following way. Langrangian as a function of pendulums are is a sum of Langranians of the pendulums. Lagrangian as a function
of can be obtained by
the substitution of by their dependences on
. So Lagrangian of
the aggregate can be obtained from the Lagrangians of the parts and constrains. Let us describe the common procedure of mechanical objects with constrains. Every Lagrangian can be represented
the following way:

where *A*(*q*) is a matrix of quadratic form. This quadratic form is the kinetic energy. In the case of the pendulum, matrix *A*(*q*) is presented below:

.

What is constrains? The number of degrees of freedom of a constrained system is less than the number of degrees of freedom of an unconstrained one. Suppose *q*_{1},
..., *q*_{n} are generalized coordinates of an unconstrained system. These coordinates depend on the generalized *p*_{1}, ..., *p*_{m}
(*m* < *n*). This dependence is noted *p*(*q*). Lagrangian of the constrained system can be expressed the following way:

#### 2.3.4 Implementation

We would like to develop software which enables us to obtain Lagrangials of a mechanical systems. We need interfaces for the Lagrange mechanical system and the constrains for this
purpose. The following interface represents all objects of Lagrange mechanics:

public interface ILagrangeMechanicalObject
{
int Dimension
{
get;
}
double[] Coordinates
{
get;
set;
}
double[] VelocityVector
{
get;
set;
}
double[,] KineticEnergyMatrix
{
get;
}
double[] LagrangianDerivaton
{
get;
}
double[] Force
{
get;
}
double PotentialEnergy
{
get;
}
void Update();
}

The following table represents the meaning of members of this interface:

Number | Member | Comment (meaning) |

1 | `Dimension` | Number of generalized coordinates |

2 | `Coordinates` | Generalized coordinates |

3 | `VelocityVector` | Vector of generalized velocity |

4 | `KineticEnergyMatrix` | Matrix of kinetic energy |

5 | `LagrangianDerivaton` | Derivation of Lagrangian by generalized
coordinates |

6 | `PotentialEnergy` | State of connection |

7 | `PotentialEnergy` | Potential energy |

8 | `Update` | Updates itself |

The following snippet contains the implementation of this interface:

public class Pendulum : ILagrangeMechanicalObject
{
#region Fields
double mass;
double r;
double J;
double[] coordinates = new double[3];
double[] velocity = new double[3];
double[,] kinteticEnergy = new double[,] {
{0, 0, 0},
{0, 0, 0},
{0, 0, 0}
};
double[] force = new double[] { 0, 0, 0 };
double[] derivation = new double[3];
const double g = 9.780318;
double potentialEnergy;
#endregion
#region Ctor
public Pendulum(double mass, double r, double J)
{
this.mass = mass;
this.r = r;
this.J = J;
}
#endregion
#region ILagrangeMechanicalObject Members
int ILagrangeMechanicalObject.Dimension
{
get
{
return 3;
}
}
double[] ILagrangeMechanicalObject.Coordinates
{
get
{
return coordinates;
}
set
{
Array.Copy(value, coordinates, coordinates.Length);
}
}
double[] ILagrangeMechanicalObject.VelocityVector
{
get
{
return velocity;
}
set
{
Array.Copy(value, velocity, velocity.Length);
}
}
double[,] ILagrangeMechanicalObject.KineticEnergyMatrix
{
get { return kinteticEnergy; }
}
double[] ILagrangeMechanicalObject.LagrangianDerivaton
{
get { return derivation; }
}
double[] ILagrangeMechanicalObject.Force
{
get { return force; }
}
double ILagrangeMechanicalObject.PotentialEnergy
{
get { return potentialEnergy; }
}
void ILagrangeMechanicalObject.Update()
{
kinteticEnergy[0, 0] = mass / 2;
kinteticEnergy[1, 1] = mass / 2;
kinteticEnergy[2, 2] = J / 2;
double s = mass * r * Math.Sin(coordinates[2]) / 2;
double c = mass * r * Math.Cos(coordinates[2]) / 2;
kinteticEnergy[0, 2] = c;
kinteticEnergy[1, 2] = s;
kinteticEnergy[2, 0] = c;
kinteticEnergy[2, 1] = s;
derivation[1] = mass * g;
derivation[2] = mass * g * r * Math.Cos(coordinates[2]);
potentialEnergy = mass * g * (coordinates[1] - r * Math.Cos(coordinates[2]));
}
#endregion
}

This snippet represents the mechanical model of the pendulum. Let us consider
the construction of a mechanical model of the constrained system of two pendulums. This problem is separated into two parts:

- Construction of a model of an unconstrained system of pendulums (usage of
`AggregatedLagrangeMechanicalObject`

class);
- Construction of a model of a constrained system from a model of unconstrained pendulums (usage of
`AggregatedLagrangeMechanicalObject`

class).

There are two classes (usage of `ConstrainedLagrangeMechanicalObject`

class).

The following snippets represent the code of these classes:

public class AggregatedLagrangeMechanicalObject : ILagrangeMechanicalObject
{
#region Fields
event Action updateAll;
double[] force;
double[] coordinates;
double[] velocity;
double[][] coord;
double[][] vel;
ILagrangeMechanicalObject[] children;
double[] lagrangianDerivaton;
int dimension = 0;
double[,] kineticEnergyMatrix;
double potentialEnergy;
#endregion
#region Ctor
public AggregatedLagrangeMechanicalObject(
ICollection<ILagrangeMechanicalObject> children,
Func<double[]> addForce)
{
ILagrangeMechanicalObject[] ch =
children.ToArray<ILagrangeMechanicalObject>();
this.children = ch;
coord = new double[ch.Length][];
vel = new double[ch.Length][];
for (int i = 0; i < ch.Length; i++)
{
ILagrangeMechanicalObject ob = ch[i];
int dim = ob.Dimension;
dimension += dim;
coord[i] = new double[dim];
vel[i] = new double[dim];
}
force = new double[dimension];
coordinates = new double[dimension];
velocity = new double[dimension];
lagrangianDerivaton = new double[dimension];
kineticEnergyMatrix = new double[dimension, dimension];
for (int i = 0; i < dimension; i++)
{
force[i] = 0;
coordinates[i] = 0;
velocity[i] = 0;
for (int j = 0; j < dimension; j++)
{
kineticEnergyMatrix[i, j] = 0;
}
}
updateAll += CommonUpdate;
if (addForce != null)
{
updateAll += () =>
{
double[] af = addForce();
for (int i = 0; i < dimension; i++)
{
force[i] += af[i];
}
};
}
}
#endregion
#region ILagrangeMechanicalObject Members
int ILagrangeMechanicalObject.Dimension
{
get { return dimension; }
}
double[] ILagrangeMechanicalObject.Coordinates
{
get
{
return coordinates;
}
set
{
Array.Copy(value, coordinates, coordinates.Length);
}
}
double[] ILagrangeMechanicalObject.VelocityVector
{
get
{
return velocity;
}
set
{
Array.Copy(value, velocity, velocity.Length);
}
}
double[,] ILagrangeMechanicalObject.KineticEnergyMatrix
{
get { return kineticEnergyMatrix; }
}
double[] ILagrangeMechanicalObject.LagrangianDerivaton
{
get { return lagrangianDerivaton; }
}
double[] ILagrangeMechanicalObject.Force
{
get { return force; }
}
double ILagrangeMechanicalObject.PotentialEnergy
{
get { return potentialEnergy; }
}
void ILagrangeMechanicalObject.Update()
{
updateAll();
}
#endregion
#region Specific Members
private void CommonUpdate()
{
int d = 0;
potentialEnergy = 0;
for (int i = 0; i < children.Length; i++)
{
ILagrangeMechanicalObject ob = children[i];
int dim = ob.Dimension;
double[] c = coord[i];
Array.Copy(coordinates, d, coord, 0, dim);
ob.Coordinates = c;
double[] v = vel[i];
Array.Copy(velocity, d, v, 0, dim);
ob.VelocityVector = v;
ob.Update();
potentialEnergy += ob.PotentialEnergy;
double[,] km = ob.KineticEnergyMatrix;
for (int k = 0; k < dim; k++) {
for (int l = 0; l < dim; l++)
{
kineticEnergyMatrix[k + d, l + d] = km[k, l];
}
}
Array.Copy(ob.Force, d, force, 0, dim);
Array.Copy(ob.LagrangianDerivaton, d, lagrangianDerivaton, 0, dim);
d += dim;
}
}
#endregion
}

public class ConstrainedLagrangeMechanicalObject : ILagrangeMechanicalObject
{
#region Fields
event Action updateAll;
double[] force;
double[] coordinates;
double[] velocity;
double[] velocityBuffer;
ILagrangeMechanicalObject unconstrainedObject;
double[] lagrangianDerivaton;
int dimension;
double[,] kineticEnergyMatrix;
#endregion
#region Ctor
public ConstrainedLagrangeMechanicalObject(ILagrangeMechanicalObject unconstrainedObject,
int dimension,
Func<double[], double[]> transformation,
Func<double[,]> partialDerivation,
Func<double[]> addForce)
{
this.unconstrainedObject = unconstrainedObject;
this.dimension = dimension;
force = new double[dimension];
coordinates = new double[dimension];
velocity = new double[dimension];
velocityBuffer = new double[unconstrainedObject.Dimension];
lagrangianDerivaton = new double[dimension];
kineticEnergyMatrix = new double[dimension, dimension];
updateAll += () =>
{
CommonUpdate(unconstrainedObject, transformation, partialDerivation);
};
if (addForce != null)
{
updateAll += () =>
{
double[] af = addForce();
for (int i = 0; i < dimension; i++)
{
force[i] += af[i];
}
};
}
}
#endregion
#region ILagrangeMechanicalObject Members
int ILagrangeMechanicalObject.Dimension
{
get { return dimension; }
}
double[] ILagrangeMechanicalObject.Coordinates
{
get
{
return coordinates;
}
set
{
Array.Copy(value, coordinates, coordinates.Length);
}
}
double[] ILagrangeMechanicalObject.VelocityVector
{
get
{
return velocity;
}
set
{
Array.Copy(value, velocity, velocity.Length);
}
}
double[,] ILagrangeMechanicalObject.KineticEnergyMatrix
{
get { return kineticEnergyMatrix; }
}
double[] ILagrangeMechanicalObject.LagrangianDerivaton
{
get { return lagrangianDerivaton; }
}
double[] ILagrangeMechanicalObject.Force
{
get { return force; }
}
double ILagrangeMechanicalObject.PotentialEnergy
{
get { return unconstrainedObject.PotentialEnergy; }
}
void ILagrangeMechanicalObject.Update()
{
updateAll();
}
#endregion
#region Members
private void CommonUpdate(ILagrangeMechanicalObject unconstrainedObject,
Func<double[], double[]> transformation,
Func<double[,]> partialDerivation)
{
double[] coord = transformation(coordinates);
unconstrainedObject.Coordinates = coord;
double[,] pd = partialDerivation(); RealMatrix.Multiply(pd, velocity, velocityBuffer);
unconstrainedObject.VelocityVector = velocityBuffer;
unconstrainedObject.Update();
RealMatrix.HtAH(pd, unconstrainedObject.KineticEnergyMatrix,
kineticEnergyMatrix); RealMatrix.Multiply(unconstrainedObject.LagrangianDerivaton,
pd, lagrangianDerivaton); RealMatrix.Multiply(unconstrainedObject.Force, pd, force);
}
#endregion
}

The following code represents the construction of a constrained system of two pendulums:

static public ILagrangeMechanicalObject ConstructTwoConstrainedPendulums(
double mass1, double r1, double J1,
double mass2, double r2, double J2, double force)
{
ILagrangeMechanicalObject[] childen = new ILagrangeMechanicalObject[] {
new Pendulum(mass1, r1, J1),
new Pendulum(mass2, r2, J2)
};
AggregatedLagrangeMechanicalObject unconstrainedAggregate =
new AggregatedLagrangeMechanicalObject(childen, null);
double[] af = new double[] { 0, force };
Func<double[]> addForce = () => {return af;};
double[] aggrCoord = new double[6]; double[,] pd = new double[,]
{{0, 0} , {0, 0}, {0, 1}, {0, 0} , {0, 0}, {0, 1}};
Func<double[,]> partialDerivation = () => { return pd;};
Func<double[], double[]> transformation = (double[] coord) =>
{
aggrCoord[0] = 0;
aggrCoord[1] = 0;
aggrCoord[2] = coord[0];
aggrCoord[3] = r1 * Math.Sin(coord[0]);
aggrCoord[4] = r1 * (1 - Math.Cos(coord[0]));
aggrCoord[5] = coord[1];
pd[3, 0] = - (r1 * Math.Sin(coord[0]));
pd[4, 0] = (r1 * Math.Cos(coord[0]));
return aggrCoord;
};
return new ConstrainedLagrangeMechanicalObject(
unconstrainedAggregate, 2,
transformation,
partialDerivation,
addForce);
}

## Points of Interest

This article will take a long time to finish and I have shared it for discussion during writing. Discussion ideas would be included in the article.

## History

To be continued...