//--------------------------------------------------------------------------------------------------
/// 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;
}
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);
}
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());
}
/**
* 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;
}
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
}
/*
* 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);
}
}
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();
}