public static Matrix3P operator *(Matrix3P a, Matrix3P b) { var res = new Matrix3P(); float x, y, z; x = b[0, 0]; y = b[1, 0]; z = b[2, 0]; res[0, 0] = a[0, 0] * x + a[0, 1] * y + a[0, 2] * z; res[1, 0] = a[1, 0] * x + a[1, 1] * y + a[1, 2] * z; res[2, 0] = a[2, 0] * x + a[2, 1] * y + a[2, 2] * z; x = b[0, 1]; y = b[1, 1]; z = b[2, 1]; res[0, 1] = a[0, 0] * x + a[0, 1] * y + a[0, 2] * z; res[1, 1] = a[1, 0] * x + a[1, 1] * y + a[1, 2] * z; res[2, 1] = a[2, 0] * x + a[2, 1] * y + a[2, 2] * z; x = b[0, 2]; y = b[1, 2]; z = b[2, 2]; res[0, 2] = a[0, 0] * x + a[0, 1] * y + a[0, 2] * z; res[1, 2] = a[1, 0] * x + a[1, 1] * y + a[1, 2] * z; res[2, 2] = a[2, 0] * x + a[2, 1] * y + a[2, 2] * z; return(res); }
/// <summary> /// for unit quaternions only? /// </summary> /// <returns></returns> public Matrix3P AsRotationMatrix() { var rotMatrix = new Matrix3P(); double xy = x * y; double wz = w * z; double wx = w * x; double wy = w * y; double xz = x * z; double yz = y * z; double zz = z * z; double yy = y * y; double xx = x * x; rotMatrix[0, 0] = (float)(1 - 2 * (yy + zz)); //1-2y2-2z2// need .997 rotMatrix[0, 1] = (float)(2 * (xy - wz)); //2xy-2wz -0.033 rotMatrix[0, 2] = (float)(2 * (xz + wy)); //// 2xz+2wy//0.063 rotMatrix[1, 0] = (float)(2 * (xy + wz)); //2xy+2wz 0.024 rotMatrix[1, 1] = (float)(1 - 2 * (xx + zz)); //1-2x2-2z2 rotMatrix[1, 2] = (float)(2 * (yz - wx)); //2yz+2wx//////////////// rotMatrix[2, 0] = (float)(2 * (xz - wy)); //2xz-2wy rotMatrix[2, 1] = (float)(2 * (yz + wx)); //2yz-2wx///////// rotMatrix[2, 2] = (float)(1 - 2 * (xx + yy)); //1-2x2-2y2 return(rotMatrix); }
private Matrix4P(Matrix3P orientation, Vector3P position) { Orientation = orientation; Position = position; }