C++ (Cpp) MatrixXf::conservativeResize Examples

Example #1

File: arm.cpp Project: IamKorra13/as4

Matrix4f Arm::rodrigues(Vector3f rot) {
        Vector3f R; R << rot(0), rot(1), rot(2);

  		if(R.norm() > 0.0f) {
  			R.normalize();
  		}
        

        Matrix3f identity;
        identity << Matrix3f::Identity();

        Matrix3f crossProd(3,3);
        crossProd(0,0) = 0.0f; crossProd(0, 1) =  -(R(0)); crossProd(0,2) = R(1);
        crossProd(1,0) = R(2); crossProd(1, 1) = 0.0; crossProd(1,2) = -(R(0));
        crossProd(2,0) = -(R(1)); crossProd(2, 1) = R(0); crossProd(2,2) = 0.0;

        Matrix3f crossProd_squ(3,3);
        crossProd_squ = crossProd * crossProd;

        float theta = rot.norm();

        MatrixXf result = identity + sin(theta) * crossProd + (1-cos(theta))*crossProd_squ;

        result.conservativeResize(4,4);
        result(0, 3) = 0.0f; result(1, 3) = 0.0f; result(2,3) = 0.0f; result(3,3) = 1.0f;
        result(3, 0) = 0.0f; result(3, 1) = 0.0f; result(3,2) = 0.0f;

        return result;
}