int main() {
  // Defines two 10x10 matrices of type long double
  // The first has elements set to 1
  // The second has elements set to 2
  Matrix<long double> mat1(10, 10, 1.0);
  Matrix<long double> mat2(10, 10, 4.0);

  mat1(3,4) = 10.0;
  mat2(1,2) = -15.0;

  // a third matrix is product of the first two

   Matrix<long double> mat3 = mat1 -mat2 ;
  save(mat3, "test.csv");

  // Print the third matrix 
  for (int i=0; i<mat3.get_rows(); i++) {
    for (int j=0; j<mat3.get_cols(); j++) {
      std::cout << mat3(i,j) << ", ";
    }
    std::cout << std::endl;
  }

  return 0;
}
void main(void)\
{\
vec3 d=normalize(o*vec3(1.,.6,1.));\
vec3 r=vec3(0.,0.,8.*p[0][1]-8.);\
float s=sin(p[0][3]*9.42);\
float c=cos(p[0][3]*9.42);\
r.xz=r.xz*mat2(c,-s,s,c);\
d.xz=d.xz*mat2(c,-s,s,c);\
s=sin(p[0][2]*9.42);\
c=cos(p[0][2]*9.42);\
r.yz=r.yz*mat2(c,-s,s,c);\
d.yz=d.yz*mat2(c,-s,s,c);\
vec3 l=vec3(0.,0.,0.);\
float m=0.;\
vec3 w=vec3(.016,.012,.009)*p[1][0];\
while(length(r)<15.&&m<0.95)\
{\
float i=min(length(r+vec3(0.,9.,0.))-8.2+20.*p[2][1],max(abs(r.y)-1.1-p[1][3]*20.,abs(length(r.xz)-2.+2.*p[1][2])*(.5+p[2][0])-2.*p[1][2]+1.2));\
vec3 n=r;\
n.y-=p[0][0]*.66;\
vec3 c=e(n*0.03,2)*.04*p[1][1];\
n.y=n.y*(.05+p[1][1])-.5*p[0][0];\
float v=e(c+n*.08,6).r;\
i-=.5*v;\
l+=w;\
m+=.003;\
float t=(1.-m)*clamp(1.-exp(i),0.,1.);\
float f=smoothstep(-2.5,-1.5,-length(r)+v*6.*(p[2][1]+1.));\
l+=(mix(vec3(.7,.2,.0),vec3(.1,.45,.85),f)*clamp(abs(f-.5),-0.5,1.)+clamp(r.y+v.x-2.*p[1][3]-1.,.0,100.))*t;\
m+=t;\
r+=d*max(.02,(i)*.5);\
}\
l+=(1.-m)*mix(vec3(.0,.1,.2),vec3(.0,.0,.1),normalize(r).y);\
gl_FragColor=vec4(l-.3,1.);\
}";
Exemple #3
0
int main(int argc, char *argv[])
{
	int a, b, c, d, e, f;

	while (scanf("%d %d %d %d %d %d", &a, &b, &c, &d, &e, &f) != EOF)
	{
		debug(printf("[DEBUG] %d %d %d %d %d %d\n", a, b, c, d, e, f));

		double delta = mat2(a, b, d, e);
		if (delta == 0)
			if (c == f)
				printf("Too many\n");
			else
				printf("No answer\n");
		else
		{
			double delta_x = mat2(c, b, f, e),
				delta_y = mat2(a, c, d, f);

			debug(printf("[DEBUG] delta=%f, delta_x=%f, delta_y=%f\n",
				delta, delta_x, delta_y));

			printf("x=%0.2f\ny=%0.2f\n",
				delta_x / delta, delta_y / delta);
		}
	}

	return 0;
}
Exemple #4
0
Actor* addModel(string filename, vec3 pos = vec3(0), vec3 s = vec3(1))
{

	Actor* sphere = new Actor;

	TransformComponent* trans = new TransformComponent();
	trans->setPos(pos);

	trans->setScale(s);
	sphere->addComponent(trans);
	glDebug();
	sphere->addComponent(new StaticMeshStaticPhysicsComponent(AssetManager::getBasePath() + filename));
	glDebug();
	MeshGraphicComponent* mesh = new MeshGraphicComponent(AssetManager::getBasePath() + filename);
	glDebug();
	mesh->setTextureMatrix(mat2(scale(mat4(1), vec3(10))));
	glDebug();
	Material *mat = new Material();
	glDebug();
	mat->setDiffuse(AssetManager::getBasePath() + "Data/Texture/floor1_d.png");
	mat->setNormal(AssetManager::getBasePath() + "Data/Texture/floor1_n.png");
	mat->setSpecular(AssetManager::getBasePath() + "Data/Texture/floor1_s.png");
	mat->setShininess(20);
	glDebug();
	mesh->addMaterial(mat);
	glDebug();
	sphere->addComponent(mesh);
	glDebug();
	return sphere;
}
Exemple #5
0
    mat2 inverse(bool *invertible=NULL) const
    {
        const float epsilon = 0.0001f;
        const float& a11 = e(0,0);
        const float& a12 = e(0,1);
        const float& a21 = e(1,0);
        const float& a22 = e(1,1);

        float DET = a11*a22-a12*a21;

        if (invertible)
            if (DET < epsilon)
                *invertible = false;
            else
                *invertible = true;

        mat2 m;
        m *= 0;

        if (DET < epsilon)
            return m;

        m = mat2(+a22, -a12, -a21, +a11) / DET;

        return m;
    }
int test_cvtColor_RGB2RGB()
{
	cv::Mat mat = cv::imread("E:/GitCode/OpenCV_Test/test_images/lena.png", 1);
	if (!mat.data) {
		std::cout << "read image fail" << std::endl;
		return -1;
	}

	int width = mat.cols;
	int height = mat.rows;

	// uchar
	fbc::Mat3BGR mat1(height, width, mat.data);
	fbc::Mat3BGR mat2(mat1);
	fbc::Mat_<uchar, 4> mat3(height, width);
	fbc::cvtColor(mat2, mat3, fbc::CV_BGR2BGRA);

	cv::Mat mat1_(height, width, CV_8UC3, mat.data);
	cv::Mat mat2_;
	mat1_.copyTo(mat2_);
	cv::Mat mat3_(height, width, CV_8UC4);
	cv::cvtColor(mat2_, mat3_, CV_BGR2BGRA);

	assert(mat3.step == mat3_.step);
	for (int y = 0; y < mat3.rows; y++) {
		const fbc::uchar* p = mat3.ptr(y);
		const uchar* p_ = mat3_.ptr(y);

		for (int x = 0; x < mat3.step; x++) {
			assert(p[x] == p_[x]);
		}
	}

	// float
	cv::Mat matf;
	mat.convertTo(matf, CV_32FC3);

	fbc::Mat_<float, 3> mat4(height, width, matf.data);
	fbc::Mat_<float, 3> mat5(mat4);
	fbc::Mat_<float, 4> mat6(height, width);
	fbc::cvtColor(mat5, mat6, fbc::CV_BGR2BGRA);

	cv::Mat mat4_(height, width, CV_32FC3, matf.data);
	cv::Mat mat5_;
	mat4_.copyTo(mat5_);
	cv::Mat mat6_(height, width, CV_32FC4);
	cv::cvtColor(mat5_, mat6_, CV_BGR2BGRA);

	assert(mat6.step == mat6_.step);
	for (int y = 0; y < mat6.rows; y++) {
		const fbc::uchar* p = mat6.ptr(y);
		const uchar* p_ = mat6_.ptr(y);

		for (int x = 0; x < mat6.step; x++) {
			assert(p[x] == p_[x]);
		}
	}

	return 0;
}
Exemple #7
0
bool GiGraphics::drawHermiteSplines(const GiContext* ctx, int count, const Point2d* knots,
                                    const Vector2d* knotvs, bool closed, bool modelUnit)
{
    if (count < 2 || !knots || !knotvs || isStopping())
        return false;
    count = mgMin(count, 0x1000);

    int i;
    Point2d pt;
    Vector2d vec;
    vector<Point2d> pxpoints;
    Matrix2d matD(S2D(xf(), modelUnit));
    Matrix2d mat2(matD / 3.f);

    pxpoints.resize(1 + (closed ? count : count - 1) * 3);
    Point2d *pxs = &pxpoints.front();

    pt = knots[0] * matD;                       // 第一个Bezier段的起点
    vec = knotvs[0] * mat2;                     // 第一个Bezier段的起始矢量
    *pxs++ = pt;                                // 产生Bezier段的起点
    for (i = 1; i < count; i++) {               // 计算每一个Bezier段
        *pxs++ = (pt += vec);                   // 产生Bezier段的第二点
        pt = knots[i] * matD;                   // Bezier段的终点
        vec = knotvs[i] * mat2;                 // Bezier段的终止矢量
        *pxs++ = pt - vec;                      // 产生Bezier段的第三点
        *pxs++ = pt;                            // 产生Bezier段的终点
    }
    if (closed) {
        *pxs++ = (pt += vec);                   // 产生Bezier段的第二点
        *pxs++ = 2 * pxpoints[0] - pxpoints[1].asVector();  // 产生Bezier段的第三点
        *pxs++ = pxpoints[0];                   // 产生Bezier段的终点
    }
    
    return rawBeziers(ctx, &pxpoints.front(), getSize(pxpoints), closed);
}
void test_eigen2_product_large()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( product(MatrixXf(ei_random<int>(1,320), ei_random<int>(1,320))) );
    CALL_SUBTEST_2( product(MatrixXd(ei_random<int>(1,320), ei_random<int>(1,320))) );
    CALL_SUBTEST_3( product(MatrixXi(ei_random<int>(1,320), ei_random<int>(1,320))) );
    CALL_SUBTEST_4( product(MatrixXcf(ei_random<int>(1,50), ei_random<int>(1,50))) );
    CALL_SUBTEST_5( product(Matrix<float,Dynamic,Dynamic,RowMajor>(ei_random<int>(1,320), ei_random<int>(1,320))) );
  }

#ifdef EIGEN_TEST_PART_6
  {
    // test a specific issue in DiagonalProduct
    int N = 1000000;
    VectorXf v = VectorXf::Ones(N);
    MatrixXf m = MatrixXf::Ones(N,3);
    m = (v+v).asDiagonal() * m;
    VERIFY_IS_APPROX(m, MatrixXf::Constant(N,3,2));
  }

  {
    // test deferred resizing in Matrix::operator=
    MatrixXf a = MatrixXf::Random(10,4), b = MatrixXf::Random(4,10), c = a;
    VERIFY_IS_APPROX((a = a * b), (c * b).eval());
  }

  {
    MatrixXf mat1(10,10); mat1.setRandom();
    MatrixXf mat2(32,10); mat2.setRandom();
    MatrixXf result = mat1.row(2)*mat2.transpose();
    VERIFY_IS_APPROX(result, (mat1.row(2)*mat2.transpose()).eval());
  }
#endif
}
bool MeshGraphicComponent::init()
{
    textureMatrix = mat2(1);

    glDebug();
    if(!shader) {
        Preprocessor *preprocessor = new Preprocessor();
        preprocessor->addVoiceToDictionary("GLSL_VERSION", int(GlobalsManager::Istance()->get("glsl_version") * 100));
        preprocessor->addVoiceToDictionary("MAX_LIGHTS", 10);

        //if(GlobalsManager::Istance()->get("glsl_version")>=3.3)
        shader = new Shader(AssetManager::getBasePath() + "Data/Shader/StaticMesh/Default/shader.vs", AssetManager::getBasePath() + "Data/Shader/NormalMap.fs", preprocessor);
        //else shader=new Shader("Data/Shader/StaticMesh/Default_120/shader.vs","Data/Shader/StaticMesh/Default_120/shader.fs");
        AssetManager::addShader(shader);
        delete preprocessor;
    }
    if(!shader_shadow) {
        Preprocessor *preprocessor = new Preprocessor();
        preprocessor->addVoiceToDictionary("GLSL_VERSION", int(GlobalsManager::Istance()->get("glsl_version") * 100));

        shader_shadow = new Shader(AssetManager::getBasePath() + "Data/Shader/StaticMesh/Shadow/shader.vs", AssetManager::getBasePath() + "Data/Shader/Shadow.fs", preprocessor);
        AssetManager::addShader(shader_shadow);
        delete preprocessor;
    }
    mesh = AssetManager::addStaticMesh(mFilename);
    glDebug();
    if(!mesh) {
        Engine::error(Engine::EString() + "unable to load mesh at : " + mFilename);
        destroy();
        return 0;
    }

    return 1;
}
Exemple #10
0
Actor* addMap()
{
	Actor* sphere = new Actor;

	TransformComponent* trans = new TransformComponent();

	trans->setPos(vec3(0, -0.01, 0));
	Rotation rot;
	rot.setEulerAngles(vec3(0, 0.785, 0));
	trans->setRotation(rot);
	sphere->addComponent(trans);
	glDebug();
	sphere->addComponent(new StaticMeshStaticPhysicsComponent(AssetManager::getBasePath() + "Data/Model/mappa4.obj"));
	glDebug();
	MeshGraphicComponent* mesh = new MeshGraphicComponent(AssetManager::getBasePath() + "Data/Model/mappa4.obj");
	glDebug();
	mesh->setTextureMatrix(mat2(scale(mat4(1), vec3(10))));
	glDebug();
	Material *mat = new Material();
	glDebug();
	mat->setDiffuse(AssetManager::getBasePath() + "Data/Texture/floor1_d.png");
	mat->setNormal(AssetManager::getBasePath() + "Data/Texture/floor1_n.png");
	mat->setSpecular(AssetManager::getBasePath() + "Data/Texture/floor1_s.png");
	mat->setShininess(20);
	glDebug();
	mesh->addMaterial(mat);
	glDebug();
	sphere->addComponent(mesh);
	glDebug();
	return sphere;
}
TEST(MathLib, GlobalDenseMatrix)
{
	const std::size_t n_rows(5);
	const std::size_t n_cols(5);
	MathLib::GlobalDenseMatrix<double, std::size_t> mat0(n_rows,n_cols);
	MathLib::GlobalDenseMatrix<double, std::size_t> mat1(n_rows,n_cols);
	MathLib::GlobalDenseMatrix<double, std::size_t> mat2(n_rows,n_cols-1);

	for (std::size_t i(0); i<n_rows; i++) {
		for (std::size_t j(0); j<n_cols; j++) {
			mat0(i,j) = 1.0 / (i+1.0+j+1.0);
		}
	}

	mat1.setZero();
	mat1 = mat0;
	for (std::size_t i(0); i<n_rows; i++) {
		for (std::size_t j(0); j<n_cols; j++) {
			ASSERT_NEAR(1.0/(i+j+2.0), mat1(i,j), std::numeric_limits<double>::epsilon());
		}
	}

	ASSERT_THROW(mat2 = mat1, std::range_error);

}
Exemple #12
0
    static void go()
    {
        CPPUNIT_ASSERT( ROWS <= 5 &&  COLS <= 5 );
        gmtl::Matrix<T, ROWS, COLS> mat1, mat2;
        T array[] = { (T)0.78, (T) 1.4,   (T) 2.9,  (T)3.45,
                      (T)4.21, (T)57.9,  (T) 65.9,  (T)74.6,
                      (T)89.2, (T)99.2,  (T) 10.9,  (T)11.9,
                      (T)12.5, (T)13.9,  (T)14.78,  (T)15.6,
                      (T)4.21, (T)57.9,  (T) 65.9,  (T)74.6,
                      (T)89.2, (T)99.2,  (T) 10.9,  (T)11.9,
                      (T)89.2, (T)99.2,  (T) 10.9,  (T)11.9
                    };
        mat1.set( array );
        mat1 = mat2;
        CPPUNIT_ASSERT( mat1 == mat2 );
        CPPUNIT_ASSERT( mat2 == mat1 );

        // Test that != works on all elements
        for (int i = 0; i < ROWS; ++i)
        {
            for (int j = 0; j < COLS; ++j)
            {
                mat2( i, j ) = (T)1221.0f;
                CPPUNIT_ASSERT(  (mat1 != mat2) );
                CPPUNIT_ASSERT( !(mat1 == mat2) );
                mat2( i, j ) = mat1( i, j ); // put it back
            }
        }

        // Test for epsilon equals working
        CPPUNIT_ASSERT( gmtl::isEqual( mat1, mat2 ) );
        CPPUNIT_ASSERT( gmtl::isEqual( mat1, mat2, (T)0.0f ) );
        CPPUNIT_ASSERT( gmtl::isEqual( mat2, mat1, (T)0.0f ) );
        CPPUNIT_ASSERT( gmtl::isEqual( mat2, mat1, (T)100000.0f ) );
        T eps = (T)10.0;
        for (int i = 0; i < ROWS; ++i)
        {
            for (int j = 0; j < COLS; ++j)
            {
                mat2( i, j ) = mat1( i, j ) - (eps / (T)2.0);
                CPPUNIT_ASSERT(  gmtl::isEqual( mat1, mat2, eps ) );
                CPPUNIT_ASSERT( !gmtl::isEqual( mat1, mat2, (T)(eps / 3.0) ) );
                mat2( i, j ) = mat1( i, j ); // put it back
            }
        }
    }
dvec2 RBFVectorFieldGenerator2D::randomVector() {
    dvec2 v(1, 0);

    auto d = theta_(mt_);
    auto c = std::cos(d);
    auto s = std::sin(d);
    return (mat2(c, s, -s, c) * v) * static_cast<float>((x_(mt_) * 2 - 1));
}
Exemple #14
0
TEST(Math, MatrixMul1) {
	Mat4 mat1(1.0f);
	Mat4 mat2(1.0f);
	Mat4 mat = mat1*mat2;
	ASSERT_EQ(1, mat.get(0,0));	ASSERT_EQ(0, mat.get(1,0));	ASSERT_EQ(0, mat.get(2,0));	ASSERT_EQ(0, mat.get(3,0));
	ASSERT_EQ(0, mat.get(0,1));	ASSERT_EQ(1, mat.get(1,1));	ASSERT_EQ(0, mat.get(2,1));	ASSERT_EQ(0, mat.get(3,1));
	ASSERT_EQ(0, mat.get(0,2));	ASSERT_EQ(0, mat.get(1,2));	ASSERT_EQ(1, mat.get(2,2));	ASSERT_EQ(0, mat.get(3,2));
	ASSERT_EQ(0, mat.get(0,3));	ASSERT_EQ(0, mat.get(1,3));	ASSERT_EQ(0, mat.get(2,3));	ASSERT_EQ(1, mat.get(3,3));
}
Exemple #15
0
mat2 twoLinkArm::jacobian(point q)
{
	double s12=std::sin(q.X()+q.Y());
	double c12=std::cos(q.X()+q.Y());
	double fJ11=-params.l1*std::sin(q.X())-params.l2*s12;
	double fJ21=params.l1*std::cos(q.X())+params.l2*c12;
	double fJ12=-params.l2*s12;
	double fJ22=params.l2*c12;
	return mat2(fJ11,fJ12,fJ21,fJ22);
}
Exemple #16
0
 void main() \
 { \
     float s = a_angle[0]; \
     float c = a_angle[1] + 1.0; \
     mat2 rotationMatrix = mat2(c, -s, s, c); \
     vec2 position = rotationMatrix * (a_position - a_center) + a_center; \
     v_texCoord = a_texCoord; \
     gl_Position = u_projection * vec4(position, 0.0, 1.0);\
     gl_PointSize = 1.0; \
 } \
Exemple #17
0
mat2_t
mat2_transpose(mat2_t m) {
	float	m00 = m.col[0].x;
	float	m10 = m.col[0].y;

	float	m01 = m.col[1].x;
	float	m11 = m.col[1].y;

	return mat2(m00, m01, m10, m11);
}
Exemple #18
0
void
TestMatrix::runSubTest17(double& res, double& expected, std::string& subTestName)
{
    Math::SymmetricMatrix<double> mat(3, 3, 5), mat1(3, 3, 7);

    Math::Matrix<double> mat2 = mat * mat1;

    res = mat2(1, 2);
    expected = mat(0, 1) * mat1(2, 0) * mat.rows();
    subTestName = "simple_symmetric_multiply";
}
Exemple #19
0
void
TestMatrix::runSubTest7(double& res, double& expected, std::string& subTestName)
{
    Math::Matrix<double> mat(2, 3, 5), mat1(3, 4, 7);

    Math::Matrix<double> mat2 = mat * mat1;

    res = mat2(1, 3);
    expected = mat(0, 0) * mat1(0, 0) * mat.cols();
    subTestName = "simple_multiply";
}
void
ColumnMajorMatrixTest::numEntries()
{
  //numEntries is tested in other functions, like after different
  //constructors to make sure the number of entries copied over correctly
  ColumnMajorMatrix mat = *two_mat;
  ColumnMajorMatrix mat2(3, 4);

  CPPUNIT_ASSERT( mat.numEntries() == 4 );
  CPPUNIT_ASSERT( mat2.numEntries() == 12 );
}
Exemple #21
0
        static void run (LinearAlgebraUnitTest& u)
        {
            const ElementType data1[] = { 1,  2, 3,  4, 5,  6, 7,  8 };
            const ElementType data2[] = { 1, -1, 3, -1, 5, -1, 7, -1 };
            const ElementType data3[] = { 0,  3, 0,  5, 0,  7, 0,  9 };

            Matrix<ElementType> mat1 (2, 4, data1);
            Matrix<ElementType> mat2 (2, 4, data2);
            Matrix<ElementType> mat3 (2, 4, data3);

            u.expect((mat1 - mat2) == mat3);
        }
Exemple #22
0
void twoLinkArm::computeInertiaCoriolis(point q, point qdot, mat2 &D, point &C)
{
	double c2=std::cos(q.Y());
	double d11=params.m1*std::pow(params.lc1,2)+params.m2*(std::pow(params.l1,2)+std::pow(params.lc2,2)+2.0*params.l1*params.lc2*c2)+params.I1+params.I2;
	double d12=params.m2*(std::pow(params.lc2,2)+params.l1*params.lc2*c2)+params.I2;
	double d21=d12;
	double d22=params.m2*std::pow(params.lc2,2)+params.I2;
	double h=params.m2*params.l1*params.lc2*std::sin(q.Y());
	
	D=mat2(d11,d12,d21,d22);
	C=point(h*qdot.Y()*(2*qdot.X()+qdot.Y()), h*std::pow(qdot.X(),2));
}
Exemple #23
0
        static void run (LinearAlgebraUnitTest& u)
        {
            const ElementType data1[] = { 1,  2, 3,  4,  5,  6,  7,  8 };
            const ElementType data2[] = { 1, -1, 3, -1,  5, -1,  7, -1 };
            const ElementType data3[] = { 50, -10, 114, -26 };

            Matrix<ElementType> mat1 (2, 4, data1);
            Matrix<ElementType> mat2 (4, 2, data2);
            Matrix<ElementType> mat3 (2, 2, data3);

            u.expect((mat1 * mat2) == mat3);
        }
Exemple #24
0
        static void run (LinearAlgebraUnitTest& u)
        {
            const ElementType data1[] = { 1,  2, 3,  4,  5,  6,  7,  8 };
            const ElementType data2[] = { 1, -1, 3, -1,  5, -1,  7, -1 };
            const ElementType data3[] = { 1, -2, 9, -4, 25, -6, 49, -8 };

            Matrix<ElementType> mat1 (2, 4, data1);
            Matrix<ElementType> mat2 (2, 4, data2);
            Matrix<ElementType> mat3 (2, 4, data3);

            u.expect (Matrix<ElementType>::hadarmard (mat1, mat2) == mat3);
        }
Exemple #25
0
mat2 mat2::operator / ( const GLfloat s ) const {
#ifdef DEBUG
	if ( std::fabs(s) < DivideByZeroTolerance ) {
		std::cerr << "[" << __FILE__ << ":" << __LINE__ << "] "
		<< "Division by zero" << std::endl;
		return mat2();
	}
#endif // DEBUG
	
	GLfloat r = GLfloat(1.0) / s;
	return *this * r;
}
Exemple #26
0
MatrixMxN<Scalar>& MatrixMxN<Scalar>::operator= (const MatrixMxN<Scalar> &mat2)
{
    unsigned int rows = (*this).rows();
    unsigned int cols = (*this).cols();
    unsigned int rows2 = mat2.rows();
    unsigned int cols2 = mat2.cols();
    if((rows != rows2)||(cols != cols2))
        (*this).resize(rows2,cols2);
    for(unsigned int i = 0; i < rows2; ++i)
        for(unsigned int j = 0; j < cols2; ++j)
            (*this)(i,j) = mat2(i,j);
    return *this;
}
Exemple #27
0
point twoLinkArm::getQDDot(point q, point qdot, point xddot)
{
	double s12=std::sin(q.X()+q.Y());
	double c12=std::cos(q.X()+q.Y());
	double fJt1dx2=-params.l1*std::cos(q.X())-params.l2*c12;
	double fJt1dy2=-params.l2*std::cos(q.X()+q.Y());
	double fJt2dx2=-params.l1*std::sin(q.X())-params.l2*s12;
	double fJt2dy2=-params.l2*s12;
	double fJt1dxdy=-params.l2*c12;
	double fJt2dxdy=-params.l2*s12;
	return jacobian(q)/(xddot-(mat2(fJt1dx2*qdot.X()+fJt1dxdy*qdot.Y(),fJt1dy2*qdot.Y()+fJt1dxdy*qdot.X(),
		fJt2dx2*qdot.X()+fJt2dxdy*qdot.Y(),fJt2dy2*qdot.Y()+fJt2dxdy*qdot.X())*qdot));
}
Exemple #28
0
void
TestMatrix::runSubTest16(double& res, double& expected, std::string& subTestName)
{
    Math::SymmetricMatrix<int> mat(3, 3);

    mat(1, 0) = -7;

    Math::Matrix<int> mat2 = mat;

    res = mat2(0, 1);
    expected = mat(0, 1);

    subTestName = "simple_matrix_from_symmetric";
}
Exemple #29
0
void
TestMatrix::runSubTest13(double& res, double& expected, std::string& subTestName)
{
    Math::SymmetricMatrix<int> mat(4, 4);

    mat(0, 1) = -7;

    Math::SymmetricMatrix<int> mat2;
    mat2 = mat;

    res = mat2(1, 0);
    expected = mat(0, 1);
    subTestName = "simple_symmetric_copy";
}
Exemple #30
0
void
TestMatrix::runSubTest6(double& res, double& expected, std::string& subTestName)
{
    Math::Matrix<int> mat(2, 3);

    mat(0, 1) = -7;

    Math::Matrix<int> mat2 = mat;
    mat2.transpose();

    res = mat2(1, 0);
    expected = mat(0, 1);
    subTestName = "simple_transpose";
}