C++ Mat3类代码示例

void Mat3::RotateZ(const float angle)
{
	Mat3 tmp;
	
	tmp.BuildRotateZ(angle);
	(*this) *= tmp;
}

void Sprite::Transform(const Mat3 &rhs)
{
	Vector3 topLeft(0 - m_anchor.x, 1 - m_anchor.y);
	Vector3 topRight(1 - m_anchor.x, 1 - m_anchor.y);
	Vector3 bottomLeft(0 - m_anchor.x, 0 - m_anchor.y);
	Vector3 bottomRight(1 - m_anchor.x, 0 - m_anchor.y);

	Mat3 scale = (scale.CreateScale( Vector3( (float)m_width, (float)m_height)));

	topLeft = rhs * (scale * topLeft);
	topRight = rhs * (scale * topRight);
	bottomLeft = rhs * (scale * bottomLeft);
	bottomRight = rhs * (scale * bottomRight);

	topLeft.x = ((2.0f / m_game->GetScreenWidth()) * topLeft.x) - 1.0f;
	topLeft.y = ((2.0f / m_game->GetScreenHeight()) * topLeft.y) - 1.0f;
	
	topRight.x = ((2.0f / m_game->GetScreenWidth()) * topRight.x) - 1.0f;
	topRight.y = ((2.0f / m_game->GetScreenHeight()) * topRight.y) - 1.0f;
	
	bottomLeft.x = ((2.0f / m_game->GetScreenWidth()) * bottomLeft.x) - 1.0f;
	bottomLeft.y = ((2.0f / m_game->GetScreenHeight()) * bottomLeft.y) - 1.0f;
	
	bottomRight.x = ((2.0f / m_game->GetScreenWidth()) * bottomRight.x) - 1.0f;
	bottomRight.y = ((2.0f / m_game->GetScreenHeight()) * bottomRight.y) - 1.0f;

	m_vertices[0].Positions[0] = topLeft.x;
	m_vertices[0].Positions[1] = topLeft.y;
	m_vertices[1].Positions[0] = topRight.x;
	m_vertices[1].Positions[1] = topRight.y;
	m_vertices[2].Positions[0] = bottomLeft.x;
	m_vertices[2].Positions[1] = bottomLeft.y;
	m_vertices[3].Positions[0] = bottomRight.x;
	m_vertices[3].Positions[1] = bottomRight.y;
}

void calc_projective (const std::vector<double>& frame_ts,
                      const std::vector<Vec4>& gyro_quat,
                      const std::vector<Vec3>& acc_trans,
                      const std::vector<double>& gyro_ts,
                      CalibrationParams calib,
                      std::vector<Mat3>& projective)
{
    int index0 = 0;
    int index1 = 0;

    size_t frame_count = frame_ts.size();

    for (int fid = 0; fid < frame_count; fid++) {
        const double ts0 = frame_ts[fid] + calib.gyro_delay;
        Quatern quat0 = interp_gyro_quatern(ts0, gyro_quat, gyro_ts, index0) + Quatern(calib.gyro_drift);

        const double ts1 = frame_ts[fid + 1] + calib.gyro_delay;
        Quatern quat1 = interp_gyro_quatern(ts1, gyro_quat, gyro_ts, index1) + Quatern(calib.gyro_drift);

        Mat3 extr0 = calc_extrinsic(quat0);
        Mat3 extr1 = calc_extrinsic(quat1);

        Mat3 intrinsic = calc_intrinsic(calib.fx, calib.fy, calib.cx, calib.cy, calib.skew);

        Mat3 extrinsic0 = rotate_coordinate_system(AXIS_X, AXIS_MINUS_Z) * extr0  * mirror_coordinate_system(AXIS_Y);
        Mat3 extrinsic1 = rotate_coordinate_system(AXIS_X, AXIS_MINUS_Z) * extr1  * mirror_coordinate_system(AXIS_Y);

        projective[fid] = intrinsic * extrinsic0 * extrinsic1.transpose() * intrinsic.inverse();
    }
}

double volume(OpenMesh::Vec3f& pointA, OpenMesh::Vec3f& pointB, OpenMesh::Vec3f& pointC)
{
	// http://www.ditutor.com/vectors/volume_tetrahedron.html
	Mat3 m = pointsToMat(pointA , pointB, pointC);
	// use minus sign or change the order of the points in above function call 
	return -m.determinant()/6.0;
}

void SpriteBatch::DrawAgent(Agent* a_agent)
{

	Vec2 pos = a_agent->m_pos;

	float width		= (float)m_agentWidth;
	float height	= (float)m_agentHeight;

	int xOff = (int)width	/ 2;
	int yOff = (int)height	/ 2;

	Vec2 tl = Vec2(-xOff,			-yOff);
	Vec2 tr = Vec2(xOff ,			-yOff);
	Vec2 br = Vec2(-xOff,			yOff);
	Vec2 bl = Vec2(xOff ,			yOff);	

	float rot = Vec2(0,1).GetAngleBetween(a_agent->m_heading);

	//create matrix for start point
	Mat3 rotMat; 	
	rotMat.Rotate(rot + 3.141592654);
	rotMat.SetTranslation(pos);


	//set the 4 vert points to correct rotation
	tl = rotMat.TransformPoint(tl);
	tr = rotMat.TransformPoint(tr);
	br = rotMat.TransformPoint(br);
	bl = rotMat.TransformPoint(bl);	


	processSprite(&tl, &tr, &bl, &br, m_agentIBO, m_agentVBO, m_texID_agent, SPRITE_COLOUR_WHITE);
}

void work() {
  PiezoTensor pt = makePiezoTensor(3.655, 2.407, 0.328, 1.894);
  MaterialTensor mt = makeMaterialTensor(19.886e10, 5.467e10, 6.799e10, 0.783e10, 23.418e10, 5.985e10, 7.209e10);

  double rho = 4642.8;
  double eps0 = 8.8542e-12;
  double exx = eps0 * 44.9;
  double ezz = eps0 * 26.7;

  Vec3 n(0, 0, 1);

  Mat3 christ = makePiezoChristoffel(pt, mt, n, exx, ezz);
  Poly3 christPoly = christ.getPoly();
  
  double g1, g2, g3;
  christPoly.solve(&g1, &g2, &g3);
  double v1 = sqrt(g1 / rho);
  double v2 = sqrt(g2 / rho);
  double v3 = sqrt(g3 / rho);

  cout << "piezo tensor: " << endl << pt << endl;
  cout << "material tensor: " << endl << mt << endl;
  cout << "christoffel matrix: " << endl << christ << endl;
  cout << "polynome " << christPoly << endl;
  cout << "velocities = " << v1 << " " << v2 << " " << v3 << endl;
}

Mat3 mxrox (double& a, Vec3& v)
 {
  // Convert eigenvalue a (eigen angle in radians) and eigenvector v
  // into a corresponding cosine rotation matrix m. 

  double q1, q2, q3, q4, q12, q22, q32, q42;
  Mat3 result;

  // calculate quaternions and their squares 
  q4 = sin ( 0.5 * a);
  q1 = v[0] * q4;
  q2 = v[1] * q4;
  q3 = v[2] * q4;
  q4 = cos (0.5 * a);
  q12 = q1 * q1;
  q22 = q2 * q2;
  q32 = q3 * q3;
  q42 = q4 * q4;

  // now get the matrix elements 
  result.assign ((q12 - q22 - q32 + q42), (2.0 * (q1*q2 + q3*q4)),
                 (2.0 * (q1*q3 - q2*q4)), (2.0 * (q1*q2 - q3*q4)),
                 (-q12 + q22 - q32 + q42),(2.0 * (q2*q3 + q1*q4)),
                 (2.0 * (q1*q3 + q2*q4)), (2.0 * (q2*q3 - q1*q4)),
                 (-q12 - q22 + q32 + q42));

  return result;
 }

TEST(Image, Convolution_MeanBoxFilter)
{
  Image<unsigned char> in(40,40);
  in.block(10,10,20,20).fill(255.f);
  Mat3 meanBoxFilterKernel;
  meanBoxFilterKernel.fill(1.f/9.f);
  Image<unsigned char> out;
  ImageConvolution(in, meanBoxFilterKernel, out);
}

		/**
		 * \brief	基本的相机坐标系，认为参考点的方向向量为z轴
		 *
		 * \param	center	参考点的位置
		 * \param	up	  	视点向上方向的向量.
		 *
		 * \return	世界坐标系转相机坐标系的矩阵
		 */
		Mat3 LookAt(const Vec3 &center, const Vec3 &up) {
			Vec3 zc = center.normalized();//向量n
			Vec3 xc = up.cross(zc).normalized();//向量u
			Vec3 yc = zc.cross(xc);//向量v
			Mat3 R;
			R.row(0) = xc;
			R.row(1) = yc;
			R.row(2) = zc;
			return R;
		}

TEST(TinyMatrix, LookAt) {
  // 简单的正交验证
  Vec3 e; e[0]= 1; e[1] = 2; e[2] = 3;
  Mat3 R = LookAt(e);//这个R是旋转矩阵，则R为正交矩阵，R与R的转置相乘为单位阵
  Mat3 I = Mat3::Identity();
  Mat3 RRT = R*R.transpose();
  Mat3 RTR = R.transpose()*R;

  EXPECT_MATRIX_NEAR(I, RRT, 1e-15);
  EXPECT_MATRIX_NEAR(I, RTR, 1e-15);
}

MatX jointFrame(const Vect3 &loc, const Vect3 &axis_, const Vect3 &ref_)
{
  Vect3 axis, ref, other;
  Mat3 R;

  axis.normalize(axis_);
  ref.displace(ref_, axis, - ref_.dot(axis));  // in case ref & axis not perp
  ref.normalize(ref);
  other.cross(axis, ref);

  R.setXcol(ref);
  R.setYcol(other);
  R.setZcol(axis);

  return MatX(R, loc);
}

int rotCode(const Mat3 &R)
{
#define ALMOST_ONE 0.9  
  int i, j;
  Vect3 axis, ref;
  Vect3 dirs[6] = 
        {Vect3::I, Vect3::I_, Vect3::J, Vect3::J_, Vect3::K , Vect3::K_};

  axis = R.zcol();
  ref  = R.xcol();
  for (i = 0; axis.dot(dirs[i]) < ALMOST_ONE; i++);
  for (j = 0; ref.dot(dirs[((i & ~1) + 2 + j) % 6]) < ALMOST_ONE; j++);
  return i * 4 + j;

#undef ALMOST_ONE
}

IGL_INLINE void igl::fit_rotations(
  const Eigen::PlainObjectBase<DerivedS> & S,
  const bool single_precision,
  Eigen::PlainObjectBase<DerivedD> & R)
{
  using namespace std;
  const int dim = S.cols();
  const int nr = S.rows()/dim;
  assert(nr * dim == S.rows());
  assert(dim == 3);

  // resize output
  R.resize(dim,dim*nr); // hopefully no op (should be already allocated)

  //std::cout<<"S=["<<std::endl<<S<<std::endl<<"];"<<std::endl;
  //MatrixXd si(dim,dim);
  Eigen::Matrix<typename DerivedS::Scalar,3,3> si;// = Eigen::Matrix3d::Identity();
  // loop over number of rotations we're computing
  for(int r = 0;r<nr;r++)
  {
    // build this covariance matrix
    for(int i = 0;i<dim;i++)
    {
      for(int j = 0;j<dim;j++)
      {
        si(i,j) = S(i*nr+r,j);
      }
    }
    typedef Eigen::Matrix<typename DerivedD::Scalar,3,3> Mat3;
    typedef Eigen::Matrix<typename DerivedD::Scalar,3,1> Vec3;
    Mat3 ri;
    if(single_precision)
    {
      polar_svd3x3(si, ri);
    }else
    {
      Mat3 ti,ui,vi;
      Vec3 _;
      igl::polar_svd(si,ri,ti,ui,_,vi);
    }
    assert(ri.determinant() >= 0);
    R.block(0,r*dim,dim,dim) = ri.block(0,0,dim,dim).transpose();
    //cout<<matlab_format(si,C_STR("si_"<<r))<<endl;
    //cout<<matlab_format(ri.transpose().eval(),C_STR("ri_"<<r))<<endl;
  }
}

 SimpleCamera(
   const Mat3 & K = Mat3::Identity(),
   const Mat3 & R = Mat3::Identity(),
   const Vec3 & t = Vec3::Zero())
   : _K(K), _R(R), _t(t)
 {
   _C = -R.transpose() * t;
   P_From_KRt(_K, _R, _t, &_P);
 }

Mat3 operator*(const Mat3& n, const Mat3& m)
{
    Mat3 A;

    for(int i=0;i<3;i++)
	for(int j=0;j<3;j++)
	    A(i,j) = n[i]*m.col(j);

    return A;
}