bool Restraint::GetCMatrix(const Plane structureOrientation, arma::mat & CN) { // Check if restraint plane differs from global plane if (structureOrientation.CompareTo(this->_orientation)) return false; // If different CN = arma::mat(6, 6, arma::fill::zeros); Vector3d drX = this->GetXDir(); Vector3d drY = this->GetYDir(); Vector3d drZ = this->GetZDir(); Vector3d strX = structureOrientation.GetX(); Vector3d strY = structureOrientation.GetY(); Vector3d strZ = structureOrientation.GetZ(); double nxx = drX.DotProduct(strX); double nxy = drY.DotProduct(strY); double nxz = drZ.DotProduct(strZ); double nyx = drX.DotProduct(strX); double nyy = drY.DotProduct(strY); double nyz = drZ.DotProduct(strZ); double nzx = drX.DotProduct(strX); double nzy = drY.DotProduct(strY); double nzz = drZ.DotProduct(strZ); // Two times for same transformations for translations (1) and rotations(2) for (int i = 0; i < 2; i++) { CN[0 + i * 3, 0 + i * 3] = nxx; CN[0 + i * 3, 1 + i * 3] = nxy; CN[0 + i * 3, 2 + i * 3] = nxz; CN[1 + i * 3, 0 + i * 3] = nyx; CN[1 + i * 3, 1 + i * 3] = nyy; CN[1 + i * 3, 2 + i * 3] = nyz; CN[2 + i * 3, 0 + i * 3] = nyx; CN[2 + i * 3, 1 + i * 3] = nyy; CN[2 + i * 3, 2 + i * 3] = nyz; } return true; }
float Vector3d::DotProduct(const Vector3d &v1, const Vector3d &v2) { return v1.DotProduct(v2); }