/// 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; }