std::pair<bool,Vector3d> Compound::Point::getVector(Manifold* space, int i) {
//std::cout << "Compound.cpp space:\t" << getPosition()->getSpace()->getType() << std::endl;
//std::cout << "Compound.cpp i:\t" << i << std::endl;
//std::cout << "Compound.cpp getPosition()->getSpace():\t" << getPosition()->getSpace() << std::endl;
//std::cout << "Compound.cpp space:\t" << space << std::endl;
if(i <= 0) {
//std::cout << "Compound.cpp i <= 0" << std::endl;
if(getPosition()->getSpace() == space) {
//std::cout << "Compound.cpp getPosition()->getSpace() == space" << std::endl;
return std::make_pair(true,getPosition()->getVector());
} else {
//std::cout << "Compound.cpp getPosition()->getSpace() != space" << std::endl;
return std::make_pair(false,Vector3d());
}
}
//std::cout << "Compound.cpp i > 0" << std::endl;
std::vector<Manifold::PortalPtr>* portals = getPosition()->getSpace()->getPortals();
for(int j=0; j<portals->size(); ++j) {
if(!(*portals)[j]->containsPoint(getPosition().get())) {
std::pair<bool,Vector3d> out = Compound::Point((*portals)[j]->teleport(getPosition())).getVector(space, i-1);
if(out.first) {
return out;
}
}
}
return std::make_pair(false,Vector3d());
}
Vector3d Compound::PointOfReference::vectorFromPointAndNearVector(Compound::PointPtr point, Vector3d vector, int i) {
if(i > 10 | vector.squaredNorm() > 10000 | vector != vector) {
//std::cout << "Compound.cpp vector:\n" << vector << std::endl;
//return vector;
return Vector3d(0,0,0);
}
Vector3d v1 = point->getPosition()->getVector();
//Vector3d v0 = point->getPosition()->getVector();
//assert(v0 == v0);
double epsilon = 0.00001;
Manifold* space = point->getPosition()->getSpace();
//std::cout << "Compound.cpp i:\t" << i << std::endl;
Vector3d v0 = this->pointFromVector(vector)->getVector(space);
Vector3d vx = this->pointFromVector(vector + Vector3d(epsilon,0,0))->getVector(space);
Vector3d vy = this->pointFromVector(vector + Vector3d(0,epsilon,0))->getVector(space);
Vector3d vz = this->pointFromVector(vector + Vector3d(0,0,epsilon))->getVector(space);
assert(vz == vz);
Matrix3d jacobean;
jacobean << vx-v0,vy-v0,vz-v0;
jacobean /= epsilon;
/*std::cout << "getPosition()->getVector():\n" << getPosition()->getVector() << std::endl;
std::cout << "vector:\n" << vector << std::endl;
std::cout << "v0:\n" << v0 << std::endl;
std::cout << "vx:\n" << vx << std::endl;
std::cout << "vy:\n" << vy << std::endl;
std::cout << "vz:\n" << vz << std::endl;*/
//std::cout << "jacobean:\n" << jacobean << std::endl;
Vector3d delta = jacobean.inverse()*(v1-v0);
//std::cout << "v1-v0:\n" << v1-v0 << std::endl;
//std::cout << "delta:\n" << delta << std::endl;
if(delta != delta) {
return Vector3d(0,0,0);
}
//std::cout << "delta.norm():\t" << delta.norm() << std::endl;
double squaredNorm = delta.squaredNorm();
double max = 5;
if(squaredNorm > max*max) {
delta = max*delta/sqrt(squaredNorm);
}
if(squaredNorm < EPSILON*EPSILON) {
/*std::cout << "v1-v0:\n" << v1-v0 << std::endl;
std::cout << "delta:\n" << delta << std::endl;
std::cout << "vector:\n" << vector << std::endl;
std::cout << "delta+vector:\n" << delta+vector << std::endl;
std::cout << "pointOfReference->vectorFromPoint(point):\n" << pointOfReference->getGeodesic(point->getPosition())->getVector() << std::endl;*/
assert(pointFromVector(delta+vector)->getPosition()->getSpace() == point->getPosition()->getSpace());
assert((pointFromVector(delta+vector)->getPosition()->getVector() - point->getPosition()->getVector()).squaredNorm() < EPSILON);
/*assert((pointOfReference->getSpace()->pointFromVector(pointOfReference, vector)->getVector() - point->getPosition()->getVector()).squaredNorm() < EPSILON);
assert((this->pointFromVector(delta+vector)->getPosition()->getVector() - pointOfReference->getSpace()->pointFromVector(pointOfReference, vector)->getVector()).squaredNorm() < EPSILON);
assert((delta+vector - pointOfReference->getSpace()->vectorFromPoint(pointOfReference, point->getPosition())).squaredNorm() < EPSILON);*/ //Only for portals that connect to themselves.
//std::cout << "i:\t" << i << std::endl;
return delta+vector;
} else {
return vectorFromPointAndNearVector(point, delta+vector, i+1);
}
}
void keyboard(unsigned char key, int mousePositionX, int mousePositionY)
{
//std::cout << key;
//std::cout.flush();
switch(key) {
case 'w':
//por->move((Vector3d() << 0,01,0).finished());
por->move(Vector3d(0,0.2,0));
break;
case 'e':
//por->move(Vector3d(1/3.,2/3.,2/3.));
//por->move(Vector3d(0.6,0,0.8));
//por->move(Vector3d(0,0,0.051));
por->rotate((Matrix3d() <<
cos(0.1), sin(0.1), 0,
-sin(0.1), cos(0.1), 0,
0, 0, 1).finished());
break;
case 'q':
//por->move(Vector3d(-1/3.,-2/3.,-2/3.));
//por->move(Vector3d(-0.6,0,-0.8));
//por->move(Vector3d(0,0,-0.051));
por->rotate((Matrix3d() <<
cos(0.1), -sin(0.1), 0,
sin(0.1), cos(0.1), 0,
0, 0, 1).finished());
break;
case 's':
por->move(Vector3d(0,-0.2,0));
break;
case 'a':
por->move(Vector3d(-0.2,0,0));
break;
case 'd':
por->move(Vector3d(0.2,0,0));
break;
case KEY_ESCAPE:
exit(0);
break;
default:
break;
}
std::cout << "Main3.cpp space:\t" << por->getPosition()->getSpace()->getType() << std::endl;
if(por->getPosition()->getSpace()->getType() == "SurfaceOfRevolution<PortalSpace2d>") {
std::cout << "Main3.cpp t:\t" << ((SurfaceOfRevolution<PortalSpace2d>::Point*) por->getPosition().get())->getT() << std::endl;
} else {
std::cout << "Main3.cpp y:\t" << ((Euclidean::Point*) por->getPosition().get())->getCoordinates()[1] << std::endl;
}
display();
}
GTEST_TEST(testIK, atlasIK) {
RigidBodyTree model(
GetDrakePath() + "/examples/Atlas/urdf/atlas_minimal_contact.urdf");
Vector2d tspan;
tspan << 0, 1;
VectorXd q0 = model.getZeroConfiguration();
// The state frame of cpp model does not match with the state frame of MATLAB
// model, since the dofname_to_dofnum is different in cpp and MATLAB
q0(2) = 0.8;
Vector3d com_lb = Vector3d::Zero();
Vector3d com_ub = Vector3d::Zero();
com_lb(2) = 0.9;
com_ub(2) = 1.0;
WorldCoMConstraint com_kc(&model, com_lb, com_ub, tspan);
std::vector<RigidBodyConstraint*> constraint_array;
constraint_array.push_back(&com_kc);
IKoptions ikoptions(&model);
VectorXd q_sol(model.number_of_positions());
q_sol.setZero();
int info = 0;
std::vector<std::string> infeasible_constraint;
inverseKin(&model, q0, q0, constraint_array.size(), constraint_array.data(),
ikoptions, &q_sol, &info, &infeasible_constraint);
printf("info = %d\n", info);
EXPECT_EQ(info, 1);
KinematicsCache<double> cache = model.doKinematics(q_sol);
Vector3d com = model.centerOfMass(cache);
printf("%5.6f\n%5.6f\n%5.6f\n", com(0), com(1), com(2));
EXPECT_TRUE(
CompareMatrices(com, Vector3d(0, 0, 1), 1e-6,
MatrixCompareType::absolute));
}
void MoleculeTest::prepareMolecule()
{
Atom *a1 = m_molecule->addAtom();
a1->setPos(Vector3d(0.0, 0.0, 0.0));
Atom *a2 = m_molecule->addAtom();
a2->setPos(Vector3d(1.5, 0.0, 0.0));
Atom *a3 = m_molecule->addAtom();
a3->setPos(Vector3d(0.0, 1.5, 0.0));
Atom *a4 = m_molecule->addAtom();
a4->setPos(Vector3d(0.0, 0.0, 1.5));
Bond *b1 = m_molecule->addBond();
b1->setAtoms(a1->id(), a2->id(), 1);
Bond *b2 = m_molecule->addBond();
b2->setAtoms(a2->id(), a3->id(), 1);
Bond *b3 = m_molecule->addBond();
b3->setAtoms(a3->id(), a4->id(), 1);
}
GTEST_TEST(testIK, iiwaIK) {
RigidBodyTree model(
GetDrakePath() + "/examples/kuka_iiwa_arm/urdf/iiwa14.urdf");
// Create a timespan for the constraints. It's not particularly
// meaningful in this test since inverseKin() only tests a single
// point, but the constructors need something.
Vector2d tspan;
tspan << 0, 1;
// Start the robot in the zero configuration (all joints zeroed,
// pointing straight up).
VectorXd q0 = model.getZeroConfiguration();
// Constrain iiwa_link_7 (the end effector) to move 0.58 on the X
// axis and down slightly (to make room for the X axis motion).
Vector3d pos_end;
pos_end << 0.58, 0, 0.77;
const double pos_tol = 0.01;
Vector3d pos_lb = pos_end - Vector3d::Constant(pos_tol);
Vector3d pos_ub = pos_end + Vector3d::Constant(pos_tol);
const int link_7_idx = model.FindBodyIndex("iiwa_link_7");
WorldPositionConstraint wpc(&model, link_7_idx,
Vector3d(0, 0, 0), pos_lb, pos_ub, tspan);
// Constrain iiwa_joint_4 between 0.9 and 1.0.
PostureConstraint pc(&model, tspan);
drake::Vector1d joint_lb(0.9);
drake::Vector1d joint_ub(1.0);
Eigen::VectorXi joint_idx(1);
joint_idx(0) = model.findJoint("iiwa_joint_4")->get_position_start_index();
pc.setJointLimits(joint_idx, joint_lb, joint_ub);
std::vector<RigidBodyConstraint*> constraint_array;
constraint_array.push_back(&wpc);
constraint_array.push_back(&pc);
IKoptions ikoptions(&model);
VectorXd q_sol(model.number_of_positions());
q_sol.setZero();
int info = 0;
std::vector<std::string> infeasible_constraint;
inverseKin(&model, q0, q0, constraint_array.size(), constraint_array.data(),
ikoptions, &q_sol, &info, &infeasible_constraint);
EXPECT_EQ(info, 1);
// Check that our constrained joint is within where we tried to constrain it.
EXPECT_GE(q_sol(joint_idx(0)), joint_lb(0));
EXPECT_LE(q_sol(joint_idx(0)), joint_ub(0));
// Check that the link we were trying to position wound up where we expected.
KinematicsCache<double> cache = model.doKinematics(q_sol);
EXPECT_TRUE(CompareMatrices(
pos_end, model.relativeTransform(cache, 0, link_7_idx).translation(),
pos_tol + 1e-6, MatrixCompareType::absolute));
}
void MoleculeTest::translate()
{
m_molecule->translate(Vector3d(1.0, 1.1, 1.2));
QCOMPARE(m_molecule->atom(0)->pos()->x(), 1.0);
QCOMPARE(m_molecule->atom(0)->pos()->y(), 1.1);
QCOMPARE(m_molecule->atom(0)->pos()->z(), 1.2);
QCOMPARE(m_molecule->atom(1)->pos()->x(), 2.5);
// Check the center was correctly updated
QCOMPARE(m_molecule->center().x(), 1.5 / 4.0 + 1.0);
QCOMPARE(m_molecule->center().y(), 1.5 / 4.0 + 1.1);
QCOMPARE(m_molecule->center().z(), 1.5 / 4.0 + 1.2);
}
Vector3d Compound::Point::getVector(Manifold* space) {
for(int i=0; i<5; ++i) {
assert(getPosition()->isInManifold());
std::pair<bool,Vector3d> out = getVector(space, i);
if(out.first) {
//std::cout << "Compound.cpp Vector:\t" << out.second << std::endl;
return out.second;
}
}
std::cout << "Compound.cpp No connection to space found." << std::endl;
return Vector3d(0,0,0);
}
std::vector<Compound::Triangle> Compound::PointOfReference::octahedron(double k) {
std::vector<Compound::Triangle> out;
Compound::PointPtr xp = pointFromVector(Vector3d(1,0,0));
Compound::PointPtr xm = pointFromVector(Vector3d(-1,0,0));
Compound::PointPtr yp = pointFromVector(Vector3d(0,1,0));
Compound::PointPtr ym = pointFromVector(Vector3d(0,-1,0));
Compound::PointPtr zp = pointFromVector(Vector3d(0,0,1));
Compound::PointPtr zm = pointFromVector(Vector3d(0,0,-1));
Triangle triangle0 = {xp,yp,zp};
Triangle triangle1 = {xp,yp,zm};
Triangle triangle2 = {xp,ym,zp};
Triangle triangle3 = {xp,ym,zm};
Triangle triangle4 = {xm,yp,zp};
Triangle triangle5 = {xm,yp,zm};
Triangle triangle6 = {xm,ym,zp};
Triangle triangle7 = {xm,ym,zm};
out.push_back(triangle0);
out.push_back(triangle1);
out.push_back(triangle2);
out.push_back(triangle3);
out.push_back(triangle4);
out.push_back(triangle5);
out.push_back(triangle6);
out.push_back(triangle7);
return out;
}
int main() {
Vector3d r0(1, 0, 0);
double t0 = 0;
double tN = 4*M_PI;
Vector3d v0(0, 0, 0);
size_t steps = 12; // h = 1.0472
double h = (tN - t0) / steps;
vector<VectorXd> r, v;
/* can't be vector<Vector3d> because casting vector<x> to vector<y> can't be
done implicitly even if x can be implicitly casted to y */
std::tie(r, v) = RK4_newton(r0, v0, t0, tN, steps, [](Vector3d r, double) { return -r; });
ofstream file1("2a.txt");
file1 << "# t \t x \t y \t z \t v_x \t v_y \t v_z" << endl;
for (size_t i = 0; i <= steps; i++) {
file1 << i*h << "\t"
<< r[i][0] << "\t" << r[i][1] << "\t" << r[i][2] << "\t"
<< v[i][0] << "\t" << v[i][1] << "\t" << v[i][2] << "\t" << endl;
}
v0 = Vector3d(0, 0.5, 0);
steps = 52; // h = 0.2618
h = (tN - t0) / steps;
r.clear();
v.clear();
std::tie(r, v) = RK4_newton(r0, v0, t0, tN, steps, [](Vector3d r, double) { return -r; });
ofstream file2("2b.txt");
file2 << "# t \t x \t y \t z \t v_x \t v_y \t v_z" << endl;
for (size_t i = 0; i <= steps; i++) {
file2 << i*h << "\t"
<< r[i][0] << "\t" << r[i][1] << "\t" << r[i][2] << "\t"
<< v[i][0] << "\t" << v[i][1] << "\t" << v[i][2] << "\t" << endl;
}
return 0;
}
bool
Mesh::loadObj(const std::string& filename, Mesh& mesh)
{
uint32_t vertCount = 0;
uint32_t faceCount = 0;
bool hasNormals = false;
bool hasTextures = false;
std::string line;
{
std::ifstream inf(filename.c_str(), std::ios::in);
if (!inf.is_open()) return false;
while ((line = nextLine(inf)).length() > 0) {
switch (line[0]) {
case 'v':
if (line[1] == ' ' || line[1] == '\t') vertCount++;
else if (line[1] == 'n') hasNormals = true;
else if (line[1] == 't') hasTextures = true;
else { return false; }
break;
case 'f':
faceCount++;
break;
default:
// Do nothing
break;
}
}
}
{
mesh.mVerts.reserve(vertCount);
mesh.mFaces.reserve(faceCount);
mesh.mNormals.reserve(vertCount);
std::ifstream inf(filename.c_str(), std::ios::in);
if (!inf.is_open()) return false;
double x, y, z;
uint32_t p, q, r;
while ((line = nextLine(inf)).length() > 0) {
switch (line[0]) {
case 'v':
if (line[1] == ' ' || line[1] == '\t') {
std::stringstream stream(line.substr(2));
stream >> x >> y >> z;
mesh.mVerts.push_back(Vector3d(x, y, z));
mesh.mBBox.add(mesh.mVerts.back());
}
break;
case 'f':
if (hasNormals && hasTextures) {
} else if (hasNormals) {
} else if (hasTextures) {
} else {
std::stringstream stream(line.substr(1));
stream >> p >> q >> r;
p--; q--; r--;
mesh.mFaces.push_back(Triangle(p, q, r));
Vector3d v1 = mesh.mVerts[q] - mesh.mVerts[p];
Vector3d v2 = mesh.mVerts[r] - mesh.mVerts[p];
mesh.mNormals.push_back(v1.cross(v2).normalized());
}
break;
default:
// Do nothing
break;
}
}
}
void MoleculeTest::setAtomPos()
{
m_molecule->setAtomPos(1, Vector3d(1.0, 2.0, 3.0));
}
void initialize ()
{
glMatrixMode(GL_PROJECTION); // select projection matrix
glViewport(0, 0, win.width, win.height); // set the viewport
glMatrixMode(GL_PROJECTION); // set matrix mode
glLoadIdentity(); // reset projection matrix
GLfloat aspect = (GLfloat) win.width / win.height;
gluPerspective(win.field_of_view_angle, aspect, win.z_near, win.z_far); // set up a perspective projection matrix
glMatrixMode(GL_MODELVIEW); // specify which matrix is the current matrix
glShadeModel( GL_SMOOTH );
glClearDepth( 1.0f ); // specify the clear value for the depth buffer
glEnable( GL_DEPTH_TEST );
glDepthFunc( GL_LEQUAL );
glHint( GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST ); // specify implementation-specific hints
glClearColor(0.0, 0.0, 0.0, 1.0); // specify clear values for the color buffers
space = new Compound();
/*SurfaceOfRevolution<PortalSpace2d>* wormhole = new SurfaceOfRevolution<PortalSpace2d>();
por = new Compound::PointOfReference(Manifold::PointOfReferencePtr(new SurfaceOfRevolution<PortalSpace2d>::PointOfReference(wormhole)));
triangleList = por->icosahedron(0.1);*/
Euclidean* euclidean0 = new Euclidean();
Euclidean* euclidean1 = new Euclidean();
Compound::PointOfReferencePtr tempPor;
Compound::PointOfReference tempPor0(Manifold::PointOfReferencePtr(new Euclidean::PointOfReference(euclidean0)));
triangleList = tempPor0.icosahedron(2);
Compound::PointOfReference tempPor1(Manifold::PointOfReferencePtr(new Euclidean::PointOfReference(euclidean1)));
std::vector<Compound::Triangle> tempTriangleList = tempPor1.icosahedron(2);
triangleList.insert(triangleList.end(), tempTriangleList.begin(), tempTriangleList.end());
//euclidean = new Euclidean();
//std::cout << "Euclidean:\t" << euclidean << std::endl;
SurfaceOfRevolution<PortalSpace2d>* wormhole = new SurfaceOfRevolution<PortalSpace2d>();
//std::cout << "Main3.cpp wormhole bottleneck:\t" << wormhole->getBottleneckCircumference() << std::endl;
//wormhole = new SurfaceOfRevolution<PortalSpace2d>();
//std::cout << "Wormhole:\t" << wormhole << std::endl;
por = new Compound::PointOfReference(Manifold::PointOfReferencePtr(new Euclidean::PointOfReference(euclidean0)));
por->move(Vector3d(0,-3,0));
//por = new Compound::PointOfReference(std::tr1::shared_ptr<Manifold::PointOfReference>(new SurfaceOfRevolution<PortalSpace2d>::PointOfReference(wormhole)));
//por->move(Vector3d(1,0,0));
SurfaceOfRevolution<PortalSpace2d>::PortalPtr wormholePortal0 = SurfaceOfRevolution<PortalSpace2d>::PortalPtr(new SurfaceOfRevolution<PortalSpace2d>::Portal(false, wormhole));
wormhole->addPortal(wormholePortal0);
SurfaceOfRevolution<PortalSpace2d>::PortalPtr wormholePortal1 = SurfaceOfRevolution<PortalSpace2d>::PortalPtr(new SurfaceOfRevolution<PortalSpace2d>::Portal(true, wormhole));
wormholePortal1->setInvert(true);
wormhole->addPortal(wormholePortal1);
//std::cout << "Main3.cpp portal radius:\t" << wormholePortal->getRadiusOfCurvature() << std::endl;
Euclidean::PortalPtr euclideanPortal0 = Euclidean::PortalPtr(new Euclidean::Portal(Vector3d(0,0,0),fabs(wormholePortal0->getRadiusOfCurvature()),euclidean0));
Euclidean::PortalPtr euclideanPortal1 = Euclidean::PortalPtr(new Euclidean::Portal(Vector3d(0,0,0),fabs(wormholePortal1->getRadiusOfCurvature()),euclidean1));
/*Euclidean::PortalPtr euclideanPortal = Euclidean::PortalPtr(new Euclidean::Portal(Vector3d(0,0,0),fabs(wormholePortal->getRadiusOfCurvature()),euclidean));
euclideanPortal->setInvert(true);*/
//Euclidean::PortalPtr euclideanPortal = Euclidean::PortalPtr(new Euclidean::Portal(Vector3d(0,-3,0),sqrt(2),euclidean));
//std::cout << "Main3.cpp wormholePortal circumference:\t" << wormholePortal->getCircumference() << std::endl;
//std::cout << "Main3.cpp euclideanPortal circumference:\t" << euclideanPortal->getCircumference() << std::endl;
//std::cout << "Main3.cpp difference:\t" << fabs(wormholePortal->getCircumference()-euclideanPortal->getCircumference()) << std::endl;
assert(fabs(wormholePortal0->getCircumference() - euclideanPortal0->getCircumference()) < EPSILON);
assert(fabs(wormholePortal1->getCircumference() - euclideanPortal1->getCircumference()) < EPSILON);
euclidean0->addPortal(euclideanPortal0);
euclidean1->addPortal(euclideanPortal1);
euclideanPortal0->setMutualExits(wormholePortal0.get());
euclideanPortal1->setMutualExits(wormholePortal1.get(), -Matrix3d::Identity());
/*Matrix3d randrot; //Randomizes the rotation.
do {
randrot.setRandom();
while((randrot*randrot.transpose()-Matrix3d::Identity()).norm() > EPSILON) {
randrot += randrot.inverse().transpose();
randrot /= 2;
}
} while(randrot.determinant() <= 0);
por->rotate(randrot);*/
//Compound::PointOfReference tempPor(Manifold::PointOfReferencePtr(new SurfaceOfRevolution<PortalSpace2d>::PointOfReference(wormhole)));
//Compound::PointOfReference tempPor(Manifold::PointOfReferencePtr(new Euclidean::PointOfReference(euclidean1)));
//triangleList = tempPor.icosahedron(0.2);
/*SurfaceOfRevolution<PortalSpace2d>* wormhole = new SurfaceOfRevolution<PortalSpace2d>();
por = new Compound::PointOfReference(std::tr1::shared_ptr<Manifold::PointOfReference>(new SurfaceOfRevolution<PortalSpace2d>::PointOfReference(wormhole)));
SurfaceOfRevolution<PortalSpace2d>::PortalPtr portal = SurfaceOfRevolution<PortalSpace2d>::PortalPtr(new SurfaceOfRevolution<PortalSpace2d>::Portal(0.1, wormhole));
portal->setExit(portal.get());
wormhole->addPortal(portal);
Euclidean* euclidean = new Euclidean();
por = new Compound::PointOfReference(std::tr1::shared_ptr<Manifold::PointOfReference>(new Euclidean::PointOfReference(euclidean)));
Manifold::Portal* portal0 = new Euclidean::Portal(Vector3d(0,0,0),1,euclidean);
Manifold::Portal* portal1 = new Euclidean::Portal(Vector3d(0,0,0),2,euclidean);
portal0->setExit(portal1);
portal1->setExit(portal0);
euclidean->addPortal(Manifold::PortalPtr(portal0));
euclidean->addPortal(Manifold::PortalPtr(portal1));*/
glColor3f(1.0f,1.0f,1.0f);
display();
/*srand(5);
while(1) { //Randomly moves for debugging. This way, I don't have to keep trying to crash it myself.
switch(rand() % 4) {
case 0:
por->move((rand()*2-1.0)/RAND_MAX*Vector3d(1,0,0));
break;
case 1:
por->move((rand()*2-1.0)/RAND_MAX*Vector3d(0,1,0));
break;
case 2:
por->move((rand()*2-1.0)/RAND_MAX*Vector3d(0,0,1));
break;
case 3:
por->move(Vector3d::Random());
break;
}
display();
}*/
}
std::vector<Compound::Triangle> Compound::PointOfReference::icosahedron(double k) {
double phi = k*(-1.+sqrt(5.))/2.;
std::vector<Compound::Triangle> out;
Compound::PointPtr va0 = pointFromVector(Vector3d(0,k,phi));
Compound::PointPtr va1 = pointFromVector(Vector3d(0,k,-phi));
Compound::PointPtr va2 = pointFromVector(Vector3d(0,-k,phi));
Compound::PointPtr va3 = pointFromVector(Vector3d(0,-k,-phi));
Compound::PointPtr vb0 = pointFromVector(Vector3d(k,phi,0));
Compound::PointPtr vb1 = pointFromVector(Vector3d(k,-phi,0));
Compound::PointPtr vb2 = pointFromVector(Vector3d(-k,phi,0));
Compound::PointPtr vb3 = pointFromVector(Vector3d(-k,-phi,0));
Compound::PointPtr vc0 = pointFromVector(Vector3d(phi,0,k));
Compound::PointPtr vc1 = pointFromVector(Vector3d(-phi,0,k));
Compound::PointPtr vc2 = pointFromVector(Vector3d(phi,0,-k));
Compound::PointPtr vc3 = pointFromVector(Vector3d(-phi,0,-k));
Triangle triangle0 = {va0,va1,vb0};
Triangle triangle1 = {va0,va1,vb2};
Triangle triangle2 = {va2,va3,vb1};
Triangle triangle3 = {va2,va3,vb3};
Triangle triangle4 = {vb0,vb1,vc0};
Triangle triangle5 = {vb0,vb1,vc2};
Triangle triangle6 = {vb2,vb3,vc1};
Triangle triangle7 = {vb2,vb3,vc3};
Triangle triangle8 = {vc0,vc1,va0};
Triangle triangle9 = {vc0,vc1,va2};
Triangle triangle10 = {vc2,vc3,va1};
Triangle triangle11 = {vc2,vc3,va3};
out.push_back(triangle0);
out.push_back(triangle1);
out.push_back(triangle2);
out.push_back(triangle3);
out.push_back(triangle4);
out.push_back(triangle5);
out.push_back(triangle6);
out.push_back(triangle7);
out.push_back(triangle8);
out.push_back(triangle9);
out.push_back(triangle10);
out.push_back(triangle11);
return out;
}
Compound::PointPtr Compound::PointOfReference::cylindrical(double r, double theta, double h) {
Compound::PointOfReferencePtr height = pointOfReferenceFromVector(Vector3d(0,0,h));
return height->pointFromVector(Vector3d(r*cos(theta),r*sin(theta),0));
}
