void Thread_RRT::getNextGoal(RRT_Node& goal, RRT_Node& goalThisIter)
{
//first, see if we are sampling randomly, or around goal
double randNum = ((double)rand()) / ((double)RAND_MAX);
if (randNum < RANDOM_SAMPLE_THRESH)
{
//goalThisIter.copyNode(goal);
//return;
setThisGoalNearGoal(goal, goalThisIter);
return;
}
//sample random goal
Matrix4d startTrans;
goal.thread->getStartTransform(startTrans);
Vector3d startPos = startTrans.corner(Eigen::TopRight,3,1);
Vector3d startTan = startTrans.corner(Eigen::TopLeft,3,1);
goalThisIter.thread = new Thread(goal.thread->length(), startPos, startTan);
goalThisIter.thread->minimize_energy_fixedPieces();
goalThisIter.setPoints();
}
//////////////////////////////////////////////////////////////////////////
// Get look vector (this is NOT a rotation!)
Vector3d cEntity::GetLookVector(void) const
{
Matrix4d m;
m.Init(Vector3d(), 0, m_Rot.x, -m_Rot.y);
Vector3d Look = m.Transform(Vector3d(0, 0, 1));
return Look;
}
void AABB::draw_mesh()
{
glColor3d(color[0],color[1],color[2]);
Vector3d n;
glPushMatrix();
glTranslated(-center[0], -center[1], -center[2]);
Matrix4d R(q_now);
Matrix4d RT = R.transpose();
glMultMatrixd(RT.ptr());
glScaled(zoom_val,zoom_val,zoom_val);
glBegin(GL_TRIANGLES);
for(unsigned int j=0;j<p.size();++j) {
glVertex3dv(p[j].ptr());
}
glEnd();
glPopMatrix();
glColor3d(color[0],color[1],color[2]);
Vector3d u=q_now*p[0]*zoom_val-center,
v=q_now*p[1]*zoom_val-center,
w=q_now*p[2]*zoom_val-center;
glBegin(GL_TRIANGLES);
glVertex3dv(u.ptr());
glVertex3dv(v.ptr());
glVertex3dv(w.ptr());
glEnd();
}
void TrfmTranslate::applyDeriv(const Dof* q, Matrix4d& m){
for(unsigned int i=0; i<mDofs.size(); i++){
if(mDofs[i] != q) m.row(i).setZero();
else m.row(i) = m.row(3);
}
m.row(3).setZero();
}
void igl::sort_triangles(
const Eigen::PlainObjectBase<DerivedV> & V,
const Eigen::PlainObjectBase<DerivedF> & F,
Eigen::PlainObjectBase<DerivedFF> & FF,
Eigen::PlainObjectBase<DerivedI> & I)
{
using namespace Eigen;
using namespace igl;
using namespace std;
// Put model, projection, and viewport matrices into double arrays
Matrix4d MV;
Matrix4d P;
glGetDoublev(GL_MODELVIEW_MATRIX, MV.data());
glGetDoublev(GL_PROJECTION_MATRIX, P.data());
if(V.cols() == 3)
{
Matrix<typename DerivedV::Scalar, DerivedV::RowsAtCompileTime,4> hV;
hV.resize(V.rows(),4);
hV.block(0,0,V.rows(),V.cols()) = V;
hV.col(3).setConstant(1);
return sort_triangles(hV,F,MV,P,FF,I);
}else
{
return sort_triangles(V,F,MV,P,FF,I);
}
}
void VRAtom::computePositions() {
computeGeo();
string g = geo;
if (AtomicStructures.count(geo) == 0) { cout << "Error: " << geo << " is invalid!\n"; return; }
vector<Matrix4d> structure = AtomicStructures[geo];
for (auto& b : bonds) {
if (b.first >= (int)structure.size()) break;
if (b.second.extra) continue;
Matrix4d T = transformation;
Matrix4d S = structure[b.first];
VRAtom* a = b.second.atom2;
if (a == 0) { // duplets
float r = 0.5*b.second.atom1->getParams().radius;
S[3] *= r; S[3][3] = 1;
T.mult(S);
T.mult(Pnt3d(1,0,0)*r, b.second.p1);
T.mult(Pnt3d(-1,-0,0)*r, b.second.p2);
continue;
}
if (a->ID <= ID) continue;
T.mult(S);
a->transformation = T;
}
}
void TrfmTranslateZ::applyDeriv(const Dof* q, Matrix4d& m){
if(mDofs[0] != q) m = Matrix4d::Zero();
else for(int i=0; i<4; i++){
if(i==A_Z) m.row(i) = m.row(3);
else m.row(i).setZero();
}
}
void Thread::printThreadInfo()
{
//print out some information
Eigen::Transform3d first_rotation_transform(Eigen::AngleAxisd(_angle_first_rot, _tangents[0]));
int numPieces = 0;
double energy = 0.0;
double twist = 0.0;
double length = 0.0;
ThreadPiece* currPiece = threadList;
Matrix4d trans = _translate_to_start*first_rotation_transform*_transform_to_start;
trans.corner(Eigen::TopLeft,3,3) *= _length;
while (currPiece != NULL)
{
numPieces++;
energy += currPiece->_length*(currPiece->_torsion*currPiece->_torsion + currPiece->_curvature*currPiece->_curvature);
//twist += currPiece->_length*currPiece->_torsion;
length += currPiece->_length;
std::cout << "length:" << currPiece->_length << std::endl;
std::cout << "curvature: " << currPiece->_curvature/_length << " torsion: " << currPiece->_torsion/_length << std::endl;
trans *= currPiece->_transform;
currPiece = currPiece->_next_segment;
}
std::cout << "Optimal thread has " << numPieces << " pieces, and a calculated energy of " << energy*_length << std::endl;
std::cout << "Final Position: \n" << trans.corner(Eigen::TopRight, 3,1) << std::endl;
std::cout << "Final Position Wanted: \n" << _positions[1] << std::endl;
std::cout << "Final Tangent: \n" << trans.corner(Eigen::TopLeft, 3, 1)/_length << std::endl;
}
Pose getDirTransform(Pose transformation) {
Matrix4d dirTransformation = transformation.asMatrix();
dirTransformation.invert();
dirTransformation.transpose();
Pose dirTransform(dirTransformation);
return dirTransform;
}
Matrix6d adjoint_se3(Matrix4d T){
Matrix6d AdjT = Matrix6d::Zero();
Matrix3d R = T.block(0,0,3,3);
AdjT.block(0,0,3,3) = R;
AdjT.block(0,3,3,3) = skew( T.block(0,3,3,1) ) * R ;
AdjT.block(3,3,3,3) = R;
return AdjT;
}
void Thread::getPoints(MatrixXd& points)
{
Eigen::Transform3d first_rotation_transform(Eigen::AngleAxisd(_angle_first_rot, _tangents[0]));
Matrix4d trans = _translate_to_start*first_rotation_transform*_transform_to_start;
trans.corner(Eigen::TopLeft,3,3) *= _length;
threadList->getPoints(points, 0.0, 1.0/((double)(points.rows()-1)), 0, trans);
}
Matrix4d rotateZ(double _angle)
{
Matrix4d m = Matrix4d::Identity();
Matrix3d r;
r = AngleAxisd(_angle, Vector3d::UnitZ());
m.corner(TopLeft, 3, 3) = r;
return m;
}
Ray Camera::GenerateRay(const Vec2f& uv, const Vec2f& auv, float time) {
Ray ray = lens->GenerateRay(uv, auv, time);
if(!xform->IsIdentity()) {
Matrix4d m = xform->GetTransform(time);
ray = Ray(m.TransformPoint(ray.E), m.TransformVector(ray.D).GetNormalized(), ray.time);
}
return ray;
}
Range3f RayTesselatedSpherePrimitive::ComputeBoundingBox(const Intervalf& time, int approximationSamples) {
Vec3f p;
float r;
Matrix4d m;
Matrix4d mi;
_ResolveSphereAttrib(p,r,m,mi);
return m.TransformBBox(ElementOperations::SphereBoundingBox(p,r));
}
//==== Get Total Transformation Matrix from Original Points ====//
Matrix4d PtCloudGeom::GetTotalTransMat()
{
Matrix4d retMat;
retMat.initMat( m_ScaleMatrix.data() );
retMat.postMult( m_ModelMatrix.data() );
return retMat;
}
Matrix4d virtuose::getPose(float f[7]) {
Matrix4d m;
Vec3d pos(f[1], f[2], f[0]);
Quaterniond q;
q.setValue(f[4], f[5], f[3]);
m.setRotate(q);
m.setTranslate(pos);
return m;
}
ClothoidPtr ClothoidFitter::getCurve() const
{
Matrix4d lhs;
lhs = _getLhs(_totalLength);
Vector4d abcd = lhs.inverse() * _rhs;
return getClothoidWithParams(abcd);
}
Matrix4d Sim3::
hat(const Vector7d & v)
{
Matrix4d Omega;
Omega.topLeftCorner<3,3>() = ScSO3::hat(v.tail<4>());
Omega.col(3).head<3>() = v.head<3>();
Omega.row(3) = Vector4d(0., 0., 0., 0.);
return Omega;
}
Point3d ViewportUtil::cameraLocal() {
float mat[16];
glGetFloatv(GL_MODELVIEW_MATRIX,mat);
Matrix4d m;
m.set(mat);
m.invert();
Point3d p(0,0,0);
m.transform(&p);
return p;
}
Matrix4d inverse_se3(Matrix4d T){
Matrix4d Tinv = Matrix4d::Identity();
Matrix3d R;
Vector3d t;
t = T.block(0,3,3,1);
R = T.block(0,0,3,3);
Tinv.block(0,0,3,3) = R.transpose();
Tinv.block(0,3,3,1) = -R.transpose() * t;
return Tinv;
}
float RayTesselatedSpherePrimitive::ComputeAverageArea(const Intervalf& time, int approximationSamples) {
Vec3f p;
float r;
Matrix4d m;
Matrix4d mi;
_ResolveSphereAttrib(p,r,m,mi);
float area = ElementOperations::SphereArea(r);
Range3f bbox(-area/2,area/2);
return m.TransformBBox(bbox).GetSize().GetLength();
}
vruiMatrix *SGVruiMatrix::preTranslated(double x, double y, double z, vruiMatrix *matrix)
{
Matrix4d translate;
translate.setIdentity();
translate.setTranslate(x, y, z);
SGVruiMatrix *osgVruiMatrix = dynamic_cast<SGVruiMatrix *>(matrix);
translate.mult(osgVruiMatrix->matrix);
this->matrix = translate;
return this;
}
void PtCloudGeom::GetSelectedPoints( vector < vec3d > &selpts )
{
Matrix4d transMat = GetTotalTransMat();
for ( int i = 0 ; i < ( int )m_Pts.size() ; i++ )
{
if ( m_Selected[i] )
{
selpts.push_back(transMat.xform( m_Pts[i] ) );
}
}
}
int main(int, char**)
{
cout.precision(3);
Matrix4d X = Matrix4d::Random(4,4);
Matrix4d A = X + X.transpose();
cout << "Here is a random symmetric 4x4 matrix:" << endl << A << endl;
Tridiagonalization<Matrix4d> triOfA(A);
Vector3d hc = triOfA.householderCoefficients();
cout << "The vector of Householder coefficients is:" << endl << hc << endl;
return 0;
}
void Window::loadFiles(){
cout << "start parsing bunny..." << endl;
Parser::parse("bunny.xyz", bunnyPos, bunnyNor, bunny_xmin, bunny_xmax, bunny_ymin, bunny_ymax, bunny_zmin, bunny_zmax);
tran_bunny.makeTranslate(-(bunny_xmin + bunny_xmax) / 2, -(bunny_ymin + bunny_ymax) / 2, -(bunny_zmin + bunny_zmax) / 2);
tran_bunny.print("translation matrix for bunny:");
cout << "bunny is loaded." << endl;
cout << "start parsing dragon..." << endl;
Parser::parse("dragon.xyz", dragonPos, dragonNor, dragon_xmin, dragon_xmax, dragon_ymin, dragon_ymax, dragon_zmin, dragon_zmax);
tran_dragon.makeTranslate(-(dragon_xmin + dragon_xmax) / 2, -(dragon_ymin + dragon_ymax) / 2, -(dragon_zmin + dragon_zmax) / 2);
tran_dragon.print("translation matrix for dragon:");
cout << "dragon is loaded." << endl;
}
void Data4Viewer::drawRGBView()
{
_pKinect->_pRGBCamera->setGLProjectionMatrix( 0.1f,100.f);
glMatrixMode ( GL_MODELVIEW );
Eigen::Affine3d tmp; tmp.setIdentity();
Matrix4d mv = btl::utility::setModelViewGLfromPrj(tmp); //mv transform X_m to X_w i.e. model coordinate to world coordinate
glLoadMatrixd( mv.data() );
_pKinect->_pRGBCamera->renderCameraInLocal(_pKinect->_pCurrFrame->_gRGB, _pGL.get(),false, NULL, 0.2f, true ); //render in model coordinate
//PRINTSTR("drawRGBView");
return;
}
/** Add a new sample to the path. If the sample time is less than the first time,
* it is added at the end. If it is greater than the last time, it is appended
* to the path. The sample is ignored if it has a time in between the first and
* last times of the path.
*/
void
CurvePlot::addSample(const CurvePlotSample& sample)
{
bool addToBack = false;
if (m_samples.empty() || sample.t > m_samples.back().t)
{
addToBack = true;
}
else if (sample.t < m_samples.front().t)
{
addToBack = false;
}
else
{
// Sample falls within range of current samples; discard it
return;
}
if (addToBack)
m_samples.push_back(sample);
else
m_samples.push_front(sample);
if (m_samples.size() > 1)
{
// Calculate a bounding radius for this segment. No point on the curve will
// be further from the start point than the bounding radius.
if (addToBack)
{
const CurvePlotSample& lastSample = m_samples[m_samples.size() - 2];
double dt = sample.t - lastSample.t;
Matrix4d coeff = cubicHermiteCoefficients(zeroExtend(lastSample.position),
zeroExtend(sample.position),
zeroExtend(lastSample.velocity * dt),
zeroExtend(sample.velocity * dt));
Vector4d extents = coeff.cwiseAbs() * Vector4d(0.0, 1.0, 1.0, 1.0);
m_samples[m_samples.size() - 1].boundingRadius = extents.norm();
}
else
{
const CurvePlotSample& nextSample = m_samples[1];
double dt = nextSample.t - sample.t;
Matrix4d coeff = cubicHermiteCoefficients(zeroExtend(sample.position),
zeroExtend(nextSample.position),
zeroExtend(sample.velocity * dt),
zeroExtend(nextSample.velocity * dt));
Vector4d extents = coeff.cwiseAbs() * Vector4d(0.0, 1.0, 1.0, 1.0);
m_samples[1].boundingRadius = extents.norm();
}
}
}
XMLNode* Keyframe::serialize() const
{
XMLElement* node = new XMLElement( "Keyframe" );
String kfId; kfId.sprintf( "%d", _id );
XMLAttribute* attr = new XMLAttribute( "id", kfId );
node->addChild( attr );
XMLElement* element;
// the pose:
element = new XMLElement( "Pose" );
Matrix4d mat;
EigenBridge::toCVT( mat, _pose.transformation() );
element->addChild( new XMLText( mat.toString() ) );
node->addChild( element );
if( hasImage() ){
if( !FileSystem::exists( "keyframe_images" ) ){
FileSystem::mkdir( "keyframe_images" );
}
String filename;
filename.sprintf( "keyframe_images/keyframe_%06d.cvtraw", _id );
_img->save( filename );
element = new XMLElement( "Image" );
element->addChild( new XMLAttribute( "file", filename ) );
node->addChild( element );
}
// measurements:
XMLElement* meas = new XMLElement( "Measurements" );
MeasurementIterator it = _featMeas.begin();
const MeasurementIterator itEnd = _featMeas.end();
XMLAttribute* featId;
String val;
while( it != itEnd ){
element = new XMLElement( "Measurement" );
// the id:
val.sprintf( "%d", it->first );
featId = new XMLAttribute( "featureId", val );
element->addChild( featId );
element->addChild( it->second.serialize() );
meas->addChild( element );
++it;
}
node->addChild( meas );
return node;
}
void VRMolecule::setLocalOrigin(int ID) {
if (atoms.count(ID) == 0) return;
uint now = VRGlobals::CURRENT_FRAME + rand();
Matrix4d m = atoms[ID]->getTransformation();
m.invert();
Matrix4d im;
MatrixLookAt( im, Vec3d(0,0,0), Vec3d(0,0,1), Vec3d(0,1,0) );
im.mult(m);
atoms[ID]->propagateTransformation(im, now);
}
PhotonSample Light::SamplePhoton(const Vec3f& sceneCenter, float sceneRadius, const Vec2f& ss, const Vec2f& sa, float time) {
PhotonSample ret;
if(xform->IsIdentity()) {
ret.le = source->SamplePhoton(sceneCenter,sceneRadius,ss,sa,time,&ret.P,&ret.wo,&ret.pdf);
} else {
Vec3f Cl = xform->GetInverseTransform(time).TransformPoint(sceneCenter);
ret.le = source->SamplePhoton(Cl,sceneRadius,ss,sa,time,&ret.P,&ret.wo,&ret.pdf);
Matrix4d m = xform->GetTransform(time);
ret.P = m.TransformPoint(ret.P);
ret.wo = m.TransformVector(ret.wo).GetNormalized();
}
return ret;
}
