//--------------------------------------------------------------------------------------------------
/// Walk forward backward along the current camera direction
/// 
/// \return  Returns true if input caused changes to the camera an false if no changes occurred
/// 
/// \remarks As this function moves the camera, it will have no effect when projection is orthographic
/// \todo    Needs better control over movement when camera position gets close to the rotation point.
//--------------------------------------------------------------------------------------------------
bool ManipulatorTrackball::walk(int posX, int posY)
{
    if (m_camera.isNull()) return false;
    if (posX == m_lastPosX && posY == m_lastPosY) return false;

    const double vpPixSizeY = m_camera->viewport()->height();
    if (vpPixSizeY <= 0) return false;

    // !!!!
    // Should be a member variable, settable as a ratio of the model bounding box/sphere
    const double minWalkTargetDistance = 1.0;

    const double ty = (m_lastPosY - posY)/vpPixSizeY;

    // Determine target distance to travel along camera direction
    // This is the distance that we will move the camera in response to a full (whole viewport) movement of the mouse
    const Vec3d camDir = m_camera->direction();
    const Vec3d vDiff = m_rotationPoint - m_walkStartCameraPos;
    double targetDist = Math::abs(camDir*vDiff);
    if (targetDist < minWalkTargetDistance) targetDist = minWalkTargetDistance;

    // Figure out movement to append
    double moveDist = ty*targetDist*m_walkSensitivity;
    Vec3d moveVec = camDir*moveDist;

    Mat4d viewMat = m_camera->viewMatrix();
    viewMat.translatePostMultiply(moveVec);
    m_camera->setViewMatrix(viewMat);

    m_lastPosX = posX;
    m_lastPosY = posY;

    return true;
}
Example #2
0
Mat4d Camera::makeTransMat()
{
    Mat4d transMat;

    Vec4d vecS = m_upVec.crossH(m_viewVec);
    double norm = m_upVec.crossH(m_viewVec).length();
    for(int i=0; i<4; i++)
    {
        vecS(i) = vecS(i)/norm;
    }
    Vec4d vecT = m_viewVec.crossH(vecS);

    transMat(0,0) = vecS(0);
    transMat(1,0) = vecS(1);
    transMat(2,0) = vecS(2);

    transMat(0,1) = vecT(0);
    transMat(1,1) = vecT(1);
    transMat(2,1) = vecT(2);

    transMat(0,2) = -m_viewVec(0);
    transMat(1,2) = -m_viewVec(1);
    transMat(2,2) = -m_viewVec(2);

    transMat(3,3) = 1;

    transMat = transMat.makeTransMat(-m_eyepoint)*transMat;    //Translation of eye point to (0,0,0)

    m_transMat = transMat;
    return transMat;
}
//--------------------------------------------------------------------------------------------------
/// Pan the camera up/down and left/right
/// 
/// \return  Returns true if input caused changes to the camera an false if no changes occurred
//--------------------------------------------------------------------------------------------------
bool ManipulatorTrackball::pan(int posX, int posY)
{
    if (m_camera.isNull()) return false;
    if (posX == m_lastPosX && posY == m_lastPosY) return false;

    const double vpPixSizeX = m_camera->viewport()->width();
    const double vpPixSizeY = m_camera->viewport()->height();
    if (vpPixSizeX <= 0 || vpPixSizeY <= 0) return false;

    // Normalized movement in screen plane 
    double tx = (posX - m_lastPosX)/vpPixSizeX;
    double ty = (posY - m_lastPosY)/vpPixSizeY;

    // Viewport size in world coordinates
    const double aspect = m_camera->aspectRatio();
    const double vpWorldSizeY = m_camera->frontPlaneFrustumHeight();
    const double vpWorldSizeX = vpWorldSizeY*aspect;

    const Vec3d camUp = m_camera->up();
    const Vec3d camRight = m_camera->right();


    Vec3d translation(0, 0, 0);

    Camera::ProjectionType projType = m_camera->projection();
    if (projType == Camera::PERSPECTIVE)
    {
        const Vec3d camPos = m_camera->position();
        const Vec3d camDir = m_camera->direction();
        const double nearPlane = m_camera->nearPlane();

        // Compute distance from camera to rotation point projected onto camera forward direction
        const Vec3d vDiff = m_rotationPoint - camPos;
        const double camRotPointDist = Math::abs(camDir*vDiff);

        Vec3d vX =  camRight*((tx*vpWorldSizeX)/nearPlane)*camRotPointDist;
        Vec3d vY =  camUp*((ty*vpWorldSizeY)/nearPlane)*camRotPointDist;
        translation = vX + vY;
    }
    else if (projType == Camera::ORTHO)
    {
        Vec3d vX =  camRight*tx*vpWorldSizeX;
        Vec3d vY =  camUp*ty*vpWorldSizeY;
        translation = vX + vY;
    }

    Mat4d viewMat = m_camera->viewMatrix();
    viewMat.translatePostMultiply(translation);
    m_camera->setViewMatrix(viewMat);

    m_lastPosX = posX;
    m_lastPosY = posY;

    return true;
}
//--------------------------------------------------------------------------------------------------
/// Set up camera/viewport and render
//--------------------------------------------------------------------------------------------------
void OverlayNavigationCube::render(OpenGLContext* oglContext, const Vec2i& position, const Vec2ui& size, bool software, const Mat4d& viewMatrix)
{
    if (size.x() <= 0 || size.y() <= 0)
    {
        return;
    }

    if (m_axis.isNull()) 
    {
        createAxisGeometry(software);
    }

    if (m_cubeGeos.size() == 0)
    {
        createCubeGeos();

        // Create the shader for the cube geometry
        ShaderProgramGenerator gen("CubeGeoShader", ShaderSourceProvider::instance());
        gen.configureStandardHeadlightColor();
        m_cubeGeoShader = gen.generate();
        m_cubeGeoShader->linkProgram(oglContext);
    }

    // Position the camera far enough away to make the axis and the text fit within the viewport
    Mat4d axisMatrix = viewMatrix;
    axisMatrix.setTranslation(Vec3d(0, 0, -2.0));

    // Setup camera
    Camera cam;
    cam.setProjectionAsPerspective(40.0, 0.05, 100.0);
    cam.setViewMatrix(axisMatrix);
    cam.setViewport(position.x(), position.y(), size.x(), size.y());

    // Setup viewport
    cam.viewport()->applyOpenGL(oglContext, Viewport::CLEAR_DEPTH);
    cam.applyOpenGL();


    // Do the actual rendering
    // -----------------------------------------------
    MatrixState matrixState(cam);
    if (software)
    {
        renderAxisImmediateMode(oglContext, matrixState);
    }
    else
    {
        renderAxis(oglContext, matrixState);
    }

    renderCubeGeos(oglContext, software, matrixState);

    renderAxisLabels(oglContext, software, matrixState);
}
Example #5
0
void CPreviewDlg::bone_ysdnm_node(ysdnm_t *dnm, struct ysdnm_srf *srf, ysdnmv_t *v0, const Mat4d &pmat){
	int i;
	ysdnmv_t *v;
/*	for(v = v0; v; v = v->next){
		const char **skipnames = v->skipnames;
		int skips = v->skips;
		for(i = 0; i < skips; i++) if(!strcmp(skipnames[i], srf->name)){
			return;
		}
	}*/
	Mat4d mat = pmat;
	for(v = v0; v; v = v->next){
		ysdnm_bone_var *bonerot = v->bonevar;
		int bones = v->bones;
		for(i = 0; i < bones; i++) if(!strcmp(bonerot[i].name, srf->name)){
			Mat4d rotmat = bonerot[i].rot.tomat4();
	/*		MAT4TRANSLATE(srf->pos[0], srf->pos[1], srf->pos[2]);*/
			Mat4d amat = mat;
			amat.translatein(srf->pos);
			amat.translatein(bonerot[i].pos);
			Mat4d rmat = amat * rotmat;
			rmat.translate(-srf->pos);
			mat = rmat;
		}
	#if 0
		if(v->fcla & (1 << srf->cla) && 2 <= srf->nst){
			double f = v->cla[srf->cla];
			avec3_t pos;
			gldTranslate3dv(srf->pos);
			for(i = 0; i < 3; i++)
				pos[i] = srf->sta[1][i] * f + srf->sta[0][i] * (1. - f);
			gldTranslate3dv(pos);
			glRotated(srf->sta[1][3] * f + srf->sta[0][3] * (1. - f), 0, -1, 0); /* heading */
			glRotated(srf->sta[1][4] * f + srf->sta[0][4] * (1. - f), -1, 0, 0); /* pitch */
			glRotated(srf->sta[1][5] * f + srf->sta[0][5] * (1. - f), 0, 0, 1); /* bank */
			gldTranslaten3dv(srf->pos);
		}
	#endif
	}
	VECNULL(&mat[12]);
	mat4translate3dv(mat, srf->pos);
	for(i = 0; i < dnm->ns; i++) if(dnm->s[main.selectbone] == srf)
		break;
	if(i != dnm->ns)
		glColor4ub(255,255,255,255);
	else
		glColor4ub(0,255,0,255);
	glBegin(GL_LINES);
	glVertex3dv(pmat.vec3(3));
	glVertex3dv(mat.vec3(3));
	glEnd();
//	callback(srf->name, hint);
//	mat4translaten3dv(mat, srf->pos);
	for(i = 0; i < srf->nch; i++){
		if(srf->cld[i].srf){
			bone_ysdnm_node(dnm, srf->cld[i].srf, v0, mat);
		}
	}
}
//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
TEST(ModelBasicListTest, MergePartsWithTransformation)
{
    Vec3fArray* verts = new Vec3fArray;
    verts->reserve(3);
    verts->add(Vec3f(0, 0, 0));
    verts->add(Vec3f(1, 0, 0));
    verts->add(Vec3f(1, 1, 0));

    Vec3fArray* norms = new Vec3fArray;
    norms->resize(3);
    norms->set(0, Vec3f::Z_AXIS);
    norms->set(1, Vec3f::Z_AXIS);
    norms->set(2, Vec3f::Z_AXIS);

    DrawableGeo* myGeo = new DrawableGeo;
    myGeo->setFromTriangleVertexArray(verts);
    myGeo->setNormalArray(norms);

    Part* myPart = new Part;
    myPart->setDrawable(myGeo);

    Part* myPart2 = new Part;
    myPart2->setDrawable(myGeo);

    ref<ModelBasicList> myModel = new ModelBasicList;
    myModel->addPart(myPart);
    myModel->addPart(myPart2);
    EXPECT_EQ(2, myModel->partCount());

    Mat4d matrix;
    matrix.setTranslation(Vec3d(10, 20, 30));
    Transform* transform = new Transform;
    transform->setLocalTransform(matrix);
    myPart2->setTransform(transform);


    myModel->mergeParts(1000, 1000);
    EXPECT_EQ(1, myModel->partCount());

    Part* mergedPart = myModel->part(0);
    DrawableGeo* mergedGeo = dynamic_cast<DrawableGeo*>(mergedPart->drawable());
    const Vec3fArray* vertices = mergedGeo->vertexArray();
    EXPECT_EQ(6, vertices->size());

    Vec3f v5 = vertices->get(5);
    EXPECT_EQ(11, v5.x());
    EXPECT_EQ(21, v5.y());
    EXPECT_EQ(30, v5.z());
}
Example #7
0
/**
* Builds a Jacobian Matrix J[]
*/
Matd Solver::computeJ(Markers handles, int cID){
	Matd J;
	J.SetSize(mModel->GetDofCount(), 4);
	J.MakeZero();

	//Get the current Node
	Marker *h = handles[cID];
	Vec4d tempPos(h->mOffset, 1.0);
	TransformNode *node = mModel->mLimbs[h->mNodeIndex];

	// Utility identity matrix
	Mat4d identity;
	identity.MakeDiag(1.0);

	// Recursively create Jacobian
	computeJ(J, node, tempPos, identity);

	return J;
}
void Refraction::setPostTransfoMat(const Mat4d& m)
{
	postTransfoMat=m;
	invertPostTransfoMat=m.inverse();
	postTransfoMatf.set(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8], m[9], m[10], m[11], m[12], m[13], m[14], m[15]);
	invertPostTransfoMatf.set(invertPostTransfoMat[0], invertPostTransfoMat[1], invertPostTransfoMat[2], invertPostTransfoMat[3],
				  invertPostTransfoMat[4], invertPostTransfoMat[5], invertPostTransfoMat[6], invertPostTransfoMat[7],
				  invertPostTransfoMat[8], invertPostTransfoMat[9], invertPostTransfoMat[10], invertPostTransfoMat[11],
				  invertPostTransfoMat[12], invertPostTransfoMat[13], invertPostTransfoMat[14], invertPostTransfoMat[15]);
}
//--------------------------------------------------------------------------------------------------
/// Rotate
/// 
/// \return  Returns true if input caused changes to the camera an false if no changes occurred
//--------------------------------------------------------------------------------------------------
bool ManipulatorTrackball::rotate(int posX, int posY)
{
    if (m_camera.isNull()) return false;
    if (posX == m_lastPosX && posY == m_lastPosY) return false;

    const double vpPixSizeX = m_camera->viewport()->width();
    const double vpPixSizeY = m_camera->viewport()->height();
    if (vpPixSizeX <= 0 || vpPixSizeY <= 0) return false;

    const double vpPosX     = posX       - static_cast<int>(m_camera->viewport()->x());
    const double vpPosY     = posY       - static_cast<int>(m_camera->viewport()->y());
    const double vpLastPosX = m_lastPosX - static_cast<int>(m_camera->viewport()->x());
    const double vpLastPosY = m_lastPosY - static_cast<int>(m_camera->viewport()->y());

    // Scale the new/last positions to the range [-1.0, 1.0] 
    double newPosX =  2.0*(vpPosX/vpPixSizeX) - 1.0;
    double newPosY =  2.0*(vpPosY/vpPixSizeY) - 1.0;
    double lastPosX = 2.0*(vpLastPosX/vpPixSizeX) - 1.0;
    double lastPosY = 2.0*(vpLastPosY/vpPixSizeY) - 1.0;

    Mat4d viewMat = m_camera->viewMatrix();

    // Compute rotation quaternion
    Quatd rotQuat = trackballRotation(lastPosX, lastPosY, newPosX, newPosY, viewMat, m_rotateSensitivity);

    // Update navigation by modifying the view matrix
    Mat4d rotMatr = rotQuat.toMatrix4();
    rotMatr.translatePostMultiply(-m_rotationPoint);
    rotMatr.translatePreMultiply(m_rotationPoint);

    viewMat = viewMat*rotMatr;
    m_camera->setViewMatrix(viewMat);

    m_lastPosX = posX;
    m_lastPosY = posY;

    return true;
}
//--------------------------------------------------------------------------------------------------
/// Compute quaternion rotation
///
/// \param  oldPosX            x coordinate of the last position of the mouse, in the range [-1.0, 1.0]
/// \param  oldPosY            y coordinate of the last position of the mouse, in the range [-1.0, 1.0]
/// \param  newPosX            x coordinate of current position of the mouse, in the range [-1.0, 1.0]
/// \param  newPosY            y coordinate of current position of the mouse, in the range [-1.0, 1.0]
/// \param  currViewMatrix     Current transformation matrix. The inverse is used when calculating the rotation
/// \param  sensitivityFactor  Mouse sensitivity factor
///
/// Simulate a track-ball. Project the points onto the virtual trackball, then figure out the axis 
/// of rotation. This is a deformed trackball-- is a trackball in the center, but is deformed into a 
/// hyperbolic sheet of rotation away from the center.  
//--------------------------------------------------------------------------------------------------
Quatd ManipulatorTrackball::trackballRotation(double oldPosX, double oldPosY, double newPosX, double newPosY, const Mat4d& currViewMatrix, double sensitivityFactor)
{
    // This particular function was chosen after trying out several variations.
    // Implemented by Gavin Bell, lots of ideas from Thant Tessman and	the August '88 
    // issue of Siggraph's "Computer Graphics," pp. 121-129.

    // This size should really be based on the distance from the center of rotation to the point on 
    // the object underneath the mouse.  That point would then track the mouse as closely as possible.  
    const double TRACKBALL_RADIUS = 0.8f;

    // Clamp to valid range
    oldPosX = Math::clamp(oldPosX, -1.0, 1.0);
    oldPosY = Math::clamp(oldPosY, -1.0, 1.0);
    newPosX = Math::clamp(newPosX, -1.0, 1.0);
    newPosY = Math::clamp(newPosY, -1.0, 1.0);

    // First, figure out z-coordinates for projection of P1 and P2 to deformed sphere
    Vec3d p1 = projectToSphere(TRACKBALL_RADIUS, oldPosX, oldPosY);
    Vec3d p2 = projectToSphere(TRACKBALL_RADIUS, newPosX, newPosY);

    // Axis of rotation is the cross product of P1 and P2
    Vec3d a = p1 ^ p2; 

    // Figure out how much to rotate around that axis.
    Vec3d d = p1 - p2;
    double t = d.length()/(2.0*TRACKBALL_RADIUS);

    // Avoid problems with out-of-control values...
    t = Math::clamp(t, -1.0, 1.0);

    double phi = 2.0*asin(t);

    // Scale by sensitivity factor
    phi *= sensitivityFactor;

    // Use inverted matrix to find rotation axis
    Mat4d invMatr = currViewMatrix.getInverted();
    a.transformVector(invMatr);

    // Get quaternion to be returned by pointer
    Quatd quat = Quatd::fromAxisAngle(a, phi);
    return quat;
}
void StelCore::updateTransformMatrices()
{
	matAltAzToEquinoxEqu = position->getRotAltAzToEquatorial(JDay);
	matEquinoxEquToAltAz = matAltAzToEquinoxEqu.transpose();

	matEquinoxEquToJ2000 = matVsop87ToJ2000 * position->getRotEquatorialToVsop87();
	matJ2000ToEquinoxEqu = matEquinoxEquToJ2000.transpose();
	matJ2000ToAltAz = matEquinoxEquToAltAz*matJ2000ToEquinoxEqu;

	matHeliocentricEclipticToEquinoxEqu = matJ2000ToEquinoxEqu * matVsop87ToJ2000 * Mat4d::translation(-position->getCenterVsop87Pos());

	// These two next have to take into account the position of the observer on the earth
	Mat4d tmp = matJ2000ToVsop87 * matEquinoxEquToJ2000 * matAltAzToEquinoxEqu;

	matAltAzToHeliocentricEcliptic =  Mat4d::translation(position->getCenterVsop87Pos()) * tmp *
						  Mat4d::translation(Vec3d(0.,0., position->getDistanceFromCenter()));

	matHeliocentricEclipticToAltAz =  Mat4d::translation(Vec3d(0.,0.,-position->getDistanceFromCenter())) * tmp.transpose() *
						  Mat4d::translation(-position->getCenterVsop87Pos());
}
//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
TEST(ModelBasicListTest, MergePartsCheckBB)
{
    Vec3fArray* verts = new Vec3fArray;
    verts->reserve(3);
    verts->add(Vec3f(0, 0, 0));
    verts->add(Vec3f(1, 0, 0));
    verts->add(Vec3f(1, 1, 0));

    Vec3fArray* norms = new Vec3fArray;
    norms->resize(3);
    norms->set(0, Vec3f::Z_AXIS);
    norms->set(1, Vec3f::Z_AXIS);
    norms->set(2, Vec3f::Z_AXIS);

    DrawableGeo* myGeo = new DrawableGeo;
    myGeo->setFromTriangleVertexArray(verts);
    myGeo->setNormalArray(norms);


    ref<ModelBasicList> myModel = new ModelBasicList;

    {
        Part* myPart = new Part;
        myPart->setDrawable(myGeo);
        Mat4d matrix;
        matrix.setTranslation(Vec3d(10, 20, 30));
        Transform* transform = new Transform;
        transform->setLocalTransform(matrix);
        myPart->setTransform(transform);
        myModel->addPart(myPart);
    }

    {
        Part* myPart2 = new Part;
        myPart2->setDrawable(myGeo);
        Mat4d matrix;
        matrix.setTranslation(Vec3d(20, 20, 30));
        Transform* transform2 = new Transform;
        transform2->setLocalTransform(matrix);
        myPart2->setTransform(transform2);
        myModel->addPart(myPart2);
    }

    {
        Part* myPart3 = new Part;
        myPart3->setDrawable(myGeo);
        Mat4d matrix;
        matrix.setTranslation(Vec3d(100, 20, 30));
        Transform* transform3 = new Transform;
        transform3->setLocalTransform(matrix);
        myPart3->setTransform(transform3);
        myModel->addPart(myPart3);
    }

    {
        Part* myPart4 = new Part;
        myPart4->setDrawable(myGeo);
        Mat4d matrix;
        matrix.setTranslation(Vec3d(110, 20, 30));
        Transform* transform4 = new Transform;
        transform4->setLocalTransform(matrix);
        myPart4->setTransform(transform4);
        myModel->addPart(myPart4);
    }

    EXPECT_EQ(4, myModel->partCount());

    myModel->mergeParts(1, 1000);
    EXPECT_EQ(4, myModel->partCount());

    myModel->mergeParts(20, 1000);
    EXPECT_EQ(2, myModel->partCount());

    myModel->mergeParts(200, 1000);
    EXPECT_EQ(1, myModel->partCount());

}
	/// Convert to 4-by-4 projection space matrix
	Mat4d matrix() const {
		Mat4d m;
		quat().toMatrix(&m[0]);
		m.set(&vec()[0], 3, 12);
		return m;
	}
Example #14
0
void SpacePlane::anim(double dt){
	WarSpace *ws = *w;
	if(!ws){
		st::anim(dt);
		return;
	}

	if(bbody && !warping){
		const btTransform &tra = bbody->getCenterOfMassTransform();
		pos = btvc(tra.getOrigin());
		rot = btqc(tra.getRotation());
		velo = btvc(bbody->getLinearVelocity());
	}

	/* forget about beaten enemy */
	if(enemy && (enemy->getHealth() <= 0. || enemy->w != w))
		enemy = NULL;

	Mat4d mat;
	transform(mat);

	if(0 < health){
		Entity *collideignore = NULL;
		if(ai){
			if(ai->control(this, dt)){
				ai->unlink(this);
				ai = NULL;
			}
			if(!w)
				return;
		}
		else if(task == sship_undock){
			if(undocktime < 0.){
				inputs.press &= ~PL_W;
				task = sship_idle;
			}
			else{
				inputs.press |= PL_W;
				undocktime -= dt;
			}
		}
		else if(controller){
		}
		else if(!enemy && task == sship_parade){
			Entity *pm = mother ? mother->e : NULL;
			if(mother){
				if(paradec == -1)
					paradec = mother->enumParadeC(mother->Frigate);
				Vec3d target, target0(1.5, -1., -1.);
				Quatd q2, q1;
				target0[0] += paradec % 10 * 300.;
				target0[2] += paradec / 10 * -300.;
				target = pm->rot.trans(target0);
				target += pm->pos;
				Vec3d dr = this->pos - target;
				if(dr.slen() < .10 * .10){
					q1 = pm->rot;
					inputs.press &= ~PL_W;
//					parking = 1;
					this->velo += dr * (-dt * .5);
					q2 = Quatd::slerp(this->rot, q1, 1. - exp(-dt));
					this->rot = q2;
				}
				else{
	//							p->throttle = dr.slen() / 5. + .01;
					steerArrival(dt, target, pm->velo, 1. / 10., 1.);
				}
			}
			else
				task = sship_idle;
		}
		else if(task == sship_idle){
			if(race != 0 /*RandomSequence((unsigned long)this + (unsigned long)(w->war_time() / .0001)).nextd() < .0001*/){
				command(&DockCommand());
			}
			inputs.press = 0;
		}
		else{
			inputs.press = 0;
		}
	}
	else{
		this->w = NULL;
		return;
	}

	st::anim(dt);

	// inputs.press is filtered in st::anim, so we put tefpol updates after it.
	for(int i = 0; i < engines.size(); i++) if(pf[i]){
		pf[i]->move(mat.vp3(engines[i]), avec3_000, cs_orangeburn.t, !(inputs.press & PL_W));
	}

//	engineHeat = approach(engineHeat, direction & PL_W ? 1.f : 0.f, dt, 0.);
	// Exponential approach is more realistic (but costs more CPU cycles)
	engineHeat = direction & PL_W ? engineHeat + (1. - engineHeat) * (1. - exp(-dt)) : engineHeat * exp(-dt);


#if 0
	if(p->pf){
		int i;
		avec3_t pos, pos0[numof(p->pf)] = {
			{.0, -.003, .045},
		};
		for(i = 0; i < numof(p->pf); i++){
			MAT4VP3(pos, mat, pos0[i]);
			MoveTefpol3D(p->pf[i], pos, avec3_000, cs_orangeburn.t, 0);
		}
	}
#endif
}
Example #15
0
/*
* Jacobian recursive worker method
*/
void Solver::computeJ(Matd &J, TransformNode *node, Vec4d &pos, Mat4d &trans) {

	// Get initial transform data
	std::vector<Transform *> transforms = node->mTransforms;

	// LEFT MATRIX
	Mat4d leftMat;
	leftMat.MakeDiag(1.0);	// identity
	for (int i = 0; i < transforms.size(); i++)
	{
		Transform *tr = transforms[i];
		// Only need to calculate if DOF
		if (tr->IsDof())
		{
			for (int j = 0; j < tr->GetDofCount(); j++)
			{
				// Get DOF
				Dof *dof = tr->GetDof(j);
				int dofId = dof->mId;

				// Add to our map
				leftMap[dofId] = leftMat;
			}
		}
		// Now update leftMat
		leftMat = leftMat * tr->GetTransform();
	}

	// RIGHT MATRIX
	Mat4d rightMat;
	rightMat.MakeDiag(1.0);	// identity
	for (int i = transforms.size()-1; i >= 0; i--)
	{
		Transform *tr = transforms[i];
		// Only need to calculate if DOF
		if (tr->IsDof())
		{
			for (int j = tr->GetDofCount()- 1; j >= 0 ; j--)
			{
				// Get DOF
				Dof *dof = tr->GetDof(j);
				int dofId = dof->mId;

				// Add to our map
				rightMap[dofId] = rightMat;
			}
		}
		// Now update leftMat
		rightMat = tr->GetTransform() * rightMat;
	}

	// JACOBIAN
	Mat4d pTrans = node->mParentTransform;

	// New identity matrix for later use
	Mat4d newTransform;
	newTransform.MakeDiag(1.0);

	for (int i = 0; i < transforms.size(); i++)
	{		
		// Check if DOF, if so compute derivative
		Transform *tr = transforms[i];
		if (tr->IsDof())
		{
			for (int j = 0; j < tr->GetDofCount(); j++)
			{
				Mat4d deriv = tr->GetDeriv(j);

				// Get row
				Dof * dof = tr->GetDof(j);
				int dofId = dof->mId;

				Mat4d leftMat = leftMap[dofId];
				Mat4d rightMat = rightMap[dofId];
				Vec4d value = pTrans * leftMat * deriv * rightMat * trans * pos;

				J[dofId] = value;
			}
		}

		newTransform = newTransform * tr->GetTransform();
	}
	// Calculate J[] values for parent if necessary
	TransformNode *parent = node->mParentNode;
	// If the parent is non-null and not the current node
	if (parent != NULL && parent != node)
	{
		newTransform = newTransform * trans;
		computeJ(J, parent, pos, newTransform);
	}
}
Example #16
0
void VDB::rotate(const ofVec3f & axis, float angle) {
	Mat4d mat;
	mat.setToRotation(Vec3R(axis.x, axis.y, axis.z), angle);
	tempTransform.rotateRad(angle, axis.x, axis.y, axis.z);
	grid->transform().postMult(mat);
}
// Set the matrix to use to post process J2000 positions before storing in the trail
void TrailGroup::setJ2000ToTrailNative(const Mat4d& m)
{
	j2000ToTrailNative=m;
	j2000ToTrailNativeInverted=m.inverse();
}