/// Computes the actual angular speed (expressed in parent coords)
public ChVector GetWvel_par()
{
ChMatrixNM <IntInterface.Three, IntInterface.Four> tempGw = ChMatrixNM <IntInterface.Three, IntInterface.Four> .NMNULL3_4; //new ChMatrixNM<IntInterface.Three, IntInterface.Four>(0);
ChFrame <Real> .SetMatrix_Gw(ref tempGw, this.coord.rot);
return(tempGw.matrix.Matr34_x_Quat(coord_dt.rot)); // ww=[Gw]*q_dt
}
/// Computes the actual angular speed (expressed in local coords)
public ChVector GetWvel_loc()
{
ChMatrixNM <IntInterface.Three, IntInterface.Four> tempGl = ChMatrixNM <IntInterface.Three, IntInterface.Four> .NMNULL3_4;// new ChMatrixNM<IntInterface.Three, IntInterface.Four>(0);
ChFrame <Real> .SetMatrix_Gl(ref tempGl, this.coord.rot);
return(tempGl.matrix.Matr34_x_Quat(coord_dt.rot)); // wl=[Gl]*q_dt
}
//
// FUNCTIONS
//
/// Reset to identity a 3x3 matrix (ones on diagonal, zero elsewhere)
/// Note: optimized, for 3x3 matrices ONLY!
public void Set33Identity()
{
nm = new ChMatrixNM <IntInterface.Three, IntInterface.Three>(0);
// nm.matrix = new ChMatrix();
this.nm.matrix.Reset();
this.nm.matrix.Set33Element(0, 0, 1);
this.nm.matrix.Set33Element(1, 1, 1);
this.nm.matrix.Set33Element(2, 2, 1);
}
/// Copy constructor
public ChConstraintTuple_3vars(ChConstraintTuple_3vars <T1> other)
{
variables_1 = other.variables_1;
variables_2 = other.variables_2;
variables_3 = other.variables_3;
Cq_1 = other.Cq_1;
Cq_2 = other.Cq_2;
Cq_3 = other.Cq_3;
Eq_1 = other.Eq_1;
Eq_2 = other.Eq_2;
Eq_3 = other.Eq_3;
}
/// The constructor which builds a 3x3 matrix given a quaternion representing rotation.
public ChMatrix33(ChQuaternion mq) //: this()
{
nm = new ChMatrixNM <IntInterface.Three, IntInterface.Three>(0);
// nm.matrix = new ChMatrix();
this.nm.matrix.rows = 3;
this.nm.matrix.columns = 3;
this.nm.matrix.address = this.nm.buffer;
/* this.address = (double*)Marshal.AllocHGlobal(this.buffer.Length * sizeof(double));
* for (int i = 0; i < this.buffer.Length; i++)
* {
* this.address[i] = this.buffer[i];
* }*/
this.Set_A_quaternion(mq);
}
/// Fills a 3x4 matrix [Fm(q)], as in [Fp(q)]*[Fm(q)]' = [A(q)]
public static void SetMatrix_Fm(ref ChMatrixNM <IntInterface.Three, IntInterface.Four> Fp, ChQuaternion mq)
{
// Debug.Assert((Fp.GetRows() == 3) && (Fp.GetColumns() == 4));
Fp.matrix[0] = mq.e1;
Fp.matrix[1] = mq.e0;
Fp.matrix[2] = mq.e3;
Fp.matrix[3] = -mq.e2;
Fp.matrix[4] = mq.e2;
Fp.matrix[5] = -mq.e3;
Fp.matrix[6] = mq.e0;
Fp.matrix[7] = mq.e1;
Fp.matrix[8] = mq.e3;
Fp.matrix[9] = mq.e2;
Fp.matrix[10] = -mq.e1;
Fp.matrix[11] = mq.e0;
}
/// Copy constructor
public ChMatrixNM(ChMatrixNM <A, B> msource)//: this()
{
this.matrix.rows = new A().Masta;
this.matrix.columns = new B().Masta;
buffer = new double[this.matrix.rows * this.matrix.columns + 3];
// for (int i = 0; i < buffer.Length; ++i)
this.matrix.address = buffer;
/*this.address = (double*)Marshal.AllocHGlobal(buffer.Length * sizeof(double));
* for (int i = 0; i < this.buffer.Length; i++)
* {
* this.address[i] = this.buffer[i];
* }*/
//this.address = buffer;
ElementsCopy(this.matrix.address, msource.matrix.address, this.matrix.rows * this.matrix.columns);
}
/// Construct a symmetric 3x3 matrix with the specified vectors for the
/// diagonal and off-diagonal. The off-diagonal vector is assumed to contain
/// the elements A(0,1), A(0,2), A(1,2) in this order.
public ChMatrix33(ChVector diag, ChVector off_diag) //: this()
{
nm = new ChMatrixNM <IntInterface.Three, IntInterface.Three>(0);
// nm.matrix = new ChMatrix();
this.nm.matrix.Set33Element(0, 0, diag.x);
this.nm.matrix.Set33Element(1, 1, diag.y);
this.nm.matrix.Set33Element(2, 2, diag.z);
this.nm.matrix.Set33Element(0, 1, off_diag.x);
this.nm.matrix.Set33Element(1, 0, off_diag.x);
this.nm.matrix.Set33Element(0, 2, off_diag.y);
this.nm.matrix.Set33Element(2, 0, off_diag.y);
this.nm.matrix.Set33Element(1, 2, off_diag.z);
this.nm.matrix.Set33Element(2, 1, off_diag.z);
}
/// Copy constructor from all types of base matrices (only with same size)
public ChMatrix33(ChMatrix msource) //: this()
{
nm = new ChMatrixNM <IntInterface.Three, IntInterface.Three>(0);
//nm.matrix = new ChMatrix();
// Debug.Assert(msource.GetColumns() == 3 && msource.GetRows() == 3);
this.nm.matrix.rows = 3;
this.nm.matrix.columns = 3;
this.nm.matrix.address = this.nm.buffer;
/* this.address = (double*)Marshal.AllocHGlobal(this.buffer.Length * sizeof(double));
* for (int i = 0; i < this.buffer.Length; i++)
* {
* this.address[i] = this.buffer[i];
* }*/
// ElementsCopy(this.address, msource.GetAddress(), 9);
for (int i = 0; i < 9; ++i)
{
this.nm.matrix.address[i] = msource.GetAddress()[i];
}
}
/// Fast fill a 3x4 matrix [Gl(q)], as in local angular speed conversion
/// Wl=[Gl]*q_dt (btw: [Gl(q)] = 2*[Fp(q')] = 2*[G] with G matrix as in Shabana)
/* public static void SetMatrix_Gl(ref ChMatrixNM<3, 4> Gl, ChQuaternion mq) {
* // assert((Gl.GetRows() == 3) && (Gl.GetColumns() == 4));
* double de0 = 2 * mq.e0;
* double de1 = 2 * mq.e1;
* double de2 = 2 * mq.e2;
* double de3 = 2 * mq.e3;
* Gl[0] = -de1;
* Gl[1] = de0;
* Gl[2] = de3;
* Gl[3] = -de2;
* Gl[4] = -de2;
* Gl[5] = -de3;
* Gl[6] = de0;
* Gl[7] = de1;
* Gl[8] = -de3;
* Gl[9] = de2;
* Gl[10] = -de1;
* Gl[11] = de0;
* }*/
/// Fast fill a 3x4 matrix [Gw(q)], as in absolute angular speed conversion
/// Ww=[Gw]*q_dt (btw: [Gw(q)] = 2*[Fm(q')] = 2*[E] with E matrix as in Shabana)
public static void SetMatrix_Gw(ref ChMatrixNM <IntInterface.Three, IntInterface.Four> Gw, ChQuaternion mq)
{
// Debug.Assert((Gw.GetRows() == 3) && (Gw.GetColumns() == 4));
double de0 = 2 * mq.e0;
double de1 = 2 * mq.e1;
double de2 = 2 * mq.e2;
double de3 = 2 * mq.e3;
Gw.matrix[0] = -de1;
Gw.matrix[1] = de0;
Gw.matrix[2] = -de3;
Gw.matrix[3] = de2;
Gw.matrix[4] = -de2;
Gw.matrix[5] = de3;
Gw.matrix[6] = de0;
Gw.matrix[7] = -de1;
Gw.matrix[8] = -de3;
Gw.matrix[9] = -de2;
Gw.matrix[10] = de1;
Gw.matrix[11] = de0;
}
// public static ChMatrix33<double> MNULL = new ChMatrix33<double>(0);
//
// CONSTRUCTORS
//
/// Default constructor builds a 3x3 matrix with zeroes.
/* public ChMatrix33() {
* }*/
/// Copy constructor
public ChMatrix33(ChMatrix33 <Real> msource)
{
nm = new ChMatrixNM <IntInterface.Three, IntInterface.Three>(0);
nm.matrix = new ChMatrix();
}
/// Copy constructor
public ChConstraintTuple_1vars(ChConstraintTuple_1vars <T> other)
{
variables = other.variables;
Cq = other.Cq;
Eq = other.Eq;
}