void TensegrityModel::addChildRotation(tgStructure& childStructure, const Yam& rotation) {
if (!rotation) return;
Yam reference = rotation["reference"];
Yam axis = rotation["axis"];
Yam angle = rotation["angle"];
if (axis && angle) {
double axisX = axis[0].as<double>();
double axisY = axis[1].as<double>();
double axisZ = axis[2].as<double>();
btVector3 axisVector = btVector3(axisX, axisY, axisZ);
double angleDegrees = angle.as<double>();
double angleRadians = tgUtil::deg2rad(angleDegrees);
btVector3 referenceVector;
if (reference) {
// rotate child around provided reference point
double referenceX = reference[0].as<double>();
double referenceY = reference[1].as<double>();
double referenceZ = reference[2].as<double>();
referenceVector = btVector3(referenceX, referenceY, referenceZ);
}
else {
// rotate child around structure's centroid
referenceVector = childStructure.getCentroid();
}
childStructure.addRotation(referenceVector, axisVector, angleRadians);
}
}
void BigDoxieNoFeet::addRodsLeg(tgStructure& s){
s.addPair(0,1,"rod");
s.addPair(1,2,"rod");
s.addPair(1,3,"rod");
s.addPair(1,4,"rod");
s.addPair(0,5,"rod");
}
void BigPuppySymmetricArching::addNodesVertebra(tgStructure& s, double r) {
s.addNode(0,0,0); //Node 0
s.addNode(r,0,r); //Node 1
s.addNode(r,0,-r); //Node 2
s.addNode(-r,0,-r); //Node 3
s.addNode(-r,0,r); //Node 4
}
/// @todo should use a best fit transformation from one set of points to another
void TensegrityModel::rotateAndTranslate(tgStructure& childStructure2,
std::vector<btVector3>& structure1RefNodes, std::vector<btVector3>& structure2RefNodes) {
btVector3 structure1RefNodesCentroid = tgUtil::getCentroid(structure1RefNodes);
btVector3 structure2RefNodesCentroid = tgUtil::getCentroid(structure2RefNodes);
btVector3 structure1PlaneNormal = ((structure1RefNodes[1] - structure1RefNodes[0]).
cross(structure1RefNodes[2] - structure1RefNodes[0])).normalize();
btVector3 structure2PlaneNormal = ((structure2RefNodes[1] - structure2RefNodes[0]).
cross(structure2RefNodes[2] - structure2RefNodes[0])).normalize();
// rotate structure 2 to align normals
btVector3 fallBackAxis = (structure2RefNodes[1] - structure2RefNodes[0]).normalize();
childStructure2.addRotation(structure2RefNodesCentroid,
tgUtil::getQuaternionBetween(structure2PlaneNormal, structure1PlaneNormal, fallBackAxis));
// rotate structure 2 ref nodes
tgUtil::addRotation(structure2RefNodes[0], structure2RefNodesCentroid,
tgUtil::getQuaternionBetween(structure2PlaneNormal, structure1PlaneNormal, fallBackAxis));
tgUtil::addRotation(structure2RefNodesCentroid, structure2RefNodesCentroid,
tgUtil::getQuaternionBetween(structure2PlaneNormal, structure1PlaneNormal, fallBackAxis));
// translate structure 2 to match up centroid points
childStructure2.move(structure1RefNodesCentroid - structure2RefNodesCentroid);
// translate structure 2 ref nodes
structure2RefNodes[0] += structure1RefNodesCentroid - structure2RefNodesCentroid;
structure2RefNodesCentroid += structure1RefNodesCentroid - structure2RefNodesCentroid;
// rotate structure 2 around structure1PlaneNormal axis to match up node with edge midpoints
childStructure2.addRotation(structure1RefNodesCentroid,
tgUtil::getQuaternionBetween(structure2RefNodes[0] - structure1RefNodesCentroid,
structure1RefNodes[0] - structure1RefNodesCentroid, structure1PlaneNormal));
}
void BigPuppySymmetricSpiralSegments::addRodsLeg(tgStructure& s){
s.addPair(0,1,"rod");
s.addPair(1,2,"rod");
s.addPair(1,3,"rod");
s.addPair(1,4,"rod");
s.addPair(0,5,"rod");
}
void BigPuppySymmetricArching::addRodsLeg(tgStructure& s) {
s.addPair(0,1,"rod");
s.addPair(1,2,"rod");
s.addPair(1,3,"rod");
s.addPair(1,4,"rod");
s.addPair(0,5,"rod");
}
void BigDoxieNoFeet::addNodesVertebra(tgStructure& s, double r){
s.addNode(0,0,0); //Node 0
s.addNode(r,0,r); //Node 1
s.addNode(r,0,-r); //Node 2
s.addNode(-r,0,-r); //Node 3
s.addNode(-r,0,r); //Node 4
}
void BigDoxieNoFeet::addNodesLeg(tgStructure& s, double r){
s.addNode(0,0,0); //0: Bottom Center of lower leg segment
s.addNode(0,r,0); //1: Center of lower leg segment
s.addNode(r,r,0); //2: Right of lower leg segment
s.addNode(-r,r,0); //3: Left of lower leg segment
s.addNode(0,2*r,0); //4: Top of lower leg segment
s.addNode(0,-r/2,0); //5: Leg segment extension for connections to foot.
}
void VerticalSpineModel::addPairsB(tgStructure& tetra)
{
tetra.addPair(0, 4, "rodB");
tetra.addPair(1, 4, "rodB");
tetra.addPair(2, 4, "rodB");
tetra.addPair(3, 4, "rodB");
}
void T6Model::addRods(tgStructure& s)
{
s.addPair( 0, 1, "rod");
s.addPair( 2, 3, "rod");
s.addPair( 4, 5, "rod");
s.addPair( 6, 7, "rod");
s.addPair( 8, 9, "rod");
s.addPair(10, 11, "rod");
}
void BigPuppySymmetricArching::addRodsFoot(tgStructure& s) {
s.addPair(0,6,"rod");
s.addPair(1,7,"rod");
s.addPair(2,4,"rod");
s.addPair(3,5,"rod");
s.addPair(0,1,"rod");
s.addPair(0,3,"rod");
s.addPair(1,2,"rod");
s.addPair(2,3,"rod");
}
void T12SuperBallPayload::addRods(tgStructure& s)
{
s.addPair( 0, 6, "r1 rod");
s.addPair( 1, 7, "r2 rod");
s.addPair( 2, 8, "r3 rod");
s.addPair( 3, 4, "r4 rod");
s.addPair( 5, 11, "r5 rod");
s.addPair( 9, 10, "r6 rod");
}
void DuCTTTestModel::addNodes(tgStructure& tetra, double edge, double height)
{
// right
tetra.addNode(-edge / 2.0, 0, tgUtil::round(std::sqrt(3.0) / 2.0 * height));
// left
tetra.addNode( edge / 2.0, 0, tgUtil::round(std::sqrt(3.0) / 2.0 * height));
// front
tetra.addNode(0, edge/2.0, 0);
// back
tetra.addNode(0, -edge/2.0, 0);
}
void TensegrityModel::addChildren(tgStructure& structure, const std::string& structurePath, tgBuildSpec& spec, const Yam& children) {
if (!children) return;
std::string structureAttributeKeys[] = {"path", "rotation", "translation", "scale", "offset"};
std::vector<std::string> structureAttributeKeysVector(structureAttributeKeys, structureAttributeKeys + sizeof(structureAttributeKeys) / sizeof(std::string));
// add all the children first
for (YAML::const_iterator child = children.begin(); child != children.end(); ++child) {
Yam childAttributes = child->second;
yamlContainsOnly(childAttributes, structurePath, structureAttributeKeysVector);
// multiple children can be defined using the syntax: child1/child2/child3...
// (add a slash so that each child is a string with a its name and a slash at the end)
std::string childCombos = child->first.as<std::string>() + "/";
while (childCombos.find("/") != std::string::npos) {
std::string childName = childCombos.substr(0, childCombos.find("/"));
addChild(structure, structurePath, childName, childAttributes["path"], spec);
childCombos = childCombos.substr(childCombos.find("/") + 1);
}
}
// apply rotation attribute to children
for (YAML::const_iterator child = children.begin(); child != children.end(); ++child) {
Yam childAttributes = child->second;
// multiple children can be defined using the syntax: child1/child2/child3...
// (add a slash so that each child is a string with a its name and a slash at the end)
std::string childCombos = child->first.as<std::string>() + "/";
while (childCombos.find("/") != std::string::npos) {
std::string childName = childCombos.substr(0, childCombos.find("/"));
tgStructure& childStructure = structure.findChild(childName);
addChildRotation(childStructure, childAttributes["rotation"]);
childCombos = childCombos.substr(childCombos.find("/") + 1);
}
}
// apply scale, offset and translation attributes to children
for (YAML::const_iterator child = children.begin(); child != children.end(); ++child) {
Yam childAttributes = child->second;
// multiple children can be defined using the syntax: child1/child2/child3...
// (add a slash so that each child is a string with a its name and a slash at the end)
std::string childCombos = child->first.as<std::string>() + "/";
int childComboIndex = 0;
while (childCombos.find("/") != std::string::npos) {
std::string childName = childCombos.substr(0, childCombos.find("/"));
tgStructure& childStructure = structure.findChild(childName);
addChildScale(childStructure, childAttributes["scale"]);
addChildOffset(childStructure, childComboIndex, childAttributes["offset"]);
addChildTranslation(childStructure, childAttributes["translation"]);
childCombos = childCombos.substr(childCombos.find("/") + 1);
childComboIndex++;
}
}
}
void VerticalSpineModel::addNodes(tgStructure& tetra, double edge, double height)
{
// right
tetra.addNode( c.edge / 2.0, 0, 0); // node 0
// left
tetra.addNode( -c.edge / 2.0, 0, 0); // node 1
// top
tetra.addNode(0, c.height, -edge / 2.0); // node 2
// front
tetra.addNode(0, c.height, edge / 2.0); // node 3
// middle
tetra.addNode(0, c.height/2, 0); // node 4
}
void TensegrityModel::addChildOffset(tgStructure& childStructure, int offsetIndex, const Yam& offset) {
if (!offset) return;
double offsetX = offset[0].as<double>();
double offsetY = offset[1].as<double>();
double offsetZ = offset[2].as<double>();
btVector3 offsetVector = btVector3(offsetX, offsetY, offsetZ);
childStructure.move(offsetVector * offsetIndex);
}
void TensegrityModel::addChildTranslation(tgStructure& childStructure, const Yam& translation) {
if (!translation) return;
double translationX = translation[0].as<double>();
double translationY = translation[1].as<double>();
double translationZ = translation[2].as<double>();
btVector3 translationVector = btVector3(translationX, translationY, translationZ);
childStructure.move(translationVector);
}
void TensegrityModel::addNodeEdgePairs(tgStructure& structure, const std::string& tags, const Yam& pairs,
const std::string* childStructure1Name, const std::string* childStructure2Name, tgBuildSpec& spec) {
if (pairs.size() < 3) {
throw std::invalid_argument("Error: node_edge bonds must specify at least 3 node_edge pairs");
}
tgStructure& childStructure1 = structure.findChild(*childStructure1Name);
tgStructure& childStructure2 = structure.findChild(*childStructure2Name);
// these are used for transformations
std::vector<btVector3> structure1RefNodes;
std::vector<btVector3> structure2RefNodes;
// these are nodes
std::vector<tgNode*> ligands;
// these are edges
std::vector< std::pair<tgNode*, tgNode*> > receptors;
// populate refNodes arrays, ligands and receptors
parseNodeEdgePairs(childStructure1, childStructure2, structure1RefNodes, structure2RefNodes, ligands, receptors, pairs);
rotateAndTranslate(childStructure2, structure1RefNodes, structure2RefNodes);
std::vector<tgBuildSpec::RigidAgent*> rigidAgents = spec.getRigidAgents();
for (unsigned int i = 0; i < ligands.size(); i++) {
// remove old edge connections
// try removing from both children since we are not sure which child the pair belongs to
// (could add more information to receptors array so we don't have to do this)
removePair(childStructure1, receptors[i].first, receptors[i].second, true, rigidAgents, spec);
removePair(childStructure2, receptors[i].first, receptors[i].second, true, rigidAgents, spec);
for (unsigned int j = 0; j < ligands.size(); j++) {
// remove old string connections between nodes/ligands
// try removing from both children since we are not sure which child the node belongs to
removePair(childStructure1, ligands[i], ligands[j], false, rigidAgents, spec);
removePair(childStructure2, ligands[i], ligands[j], false, rigidAgents, spec);
}
// make new connection from edge -> node -> edge
structure.addPair(*(receptors[i].first), *ligands[i], tags);
structure.addPair(*ligands[i], *(receptors[i].second), tags);
}
}
void T12SuperBallPayload::addMuscles(tgStructure& s)
{
int muscleConnections[13][13];
musclesPerNodes.resize(13);
for(int i=0;i<13;i++)
{
musclesPerNodes[i].resize(13);
for(int j=0;j<13;j++)
musclesPerNodes[i][j]=NULL;
}
for(int i=0;i<13;i++)
for(int j=0;j<13;j++)
muscleConnections[i][j]=-1;
muscleConnections[0][3]=0;
muscleConnections[3][2]=0;
muscleConnections[2][0]=0;
muscleConnections[4][5]=0;
muscleConnections[5][7]=0;
muscleConnections[7][4]=0;
muscleConnections[1][8]=0;
muscleConnections[8][10]=0;
muscleConnections[10][1]=0;
muscleConnections[9][11]=0;
muscleConnections[11][6]=0;
muscleConnections[6][9]=0;
muscleConnections[1][2]=1;
muscleConnections[2][4]=1;
muscleConnections[4][1]=1;
muscleConnections[3][5]=1;
muscleConnections[5][6]=1;
muscleConnections[6][3]=1;
muscleConnections[0][8]=1;
muscleConnections[8][9]=1;
muscleConnections[9][0]=1;
muscleConnections[11][7]=1;
muscleConnections[7][10]=1;
muscleConnections[10][11]=1;
for(int i=0;i<13;i++)
{
for(int j=0;j<13;j++)
{
if(muscleConnections[i][j]>=0)
{
std::stringstream tag;
tag<<"muscle-"<<i<<"-"<<j;
s.addPair(i, j, "muscle");
//musclesPerNodes[i][j]=s.addPair(i, j, tag);
//musclesPerNodes[j][i]=musclesPerNodes[i][j];
}
}
}
}
void TensegrityModel::addNodeNodePairs(tgStructure& structure, const std::string& tags, const Yam& pairs,
const std::string* childStructure1Name, const std::string* childStructure2Name) {
for (YAML::const_iterator pairPtr = pairs.begin(); pairPtr != pairs.end(); ++pairPtr) {
Yam pair = *pairPtr;
std::string node1Path = pair[0].as<std::string>();
std::string node2Path = pair[1].as<std::string>();
tgNode* node1;
tgNode* node2;
if (childStructure1Name && childStructure2Name) {
node1 = &getNode(structure.findChild(*childStructure1Name), node1Path);
node2 = &getNode(structure.findChild(*childStructure2Name), node2Path);
}
else {
node1 = &getNode(structure, node1Path);
node2 = &getNode(structure, node2Path);
}
structure.addPair(*node1, *node2, tags);
}
}
void TensegrityModel::addNodes(tgStructure& structure, const Yam& nodes) {
if (!nodes) return;
for (YAML::const_iterator node = nodes.begin(); node != nodes.end(); ++node) {
std::string name = node->first.as<std::string>();
Yam xyz = node->second;
double x = xyz[0].as<double>();
double y = xyz[1].as<double>();
double z = xyz[2].as<double>();
structure.addNode(x, y, z, name);
}
}
void BigDoxieNoFeet::addNodesFoot(tgStructure& s, double r1, double r2){
s.addNode(r2,0,r2);//0
s.addNode(r2,0,-r2);//1
s.addNode(-r2,0,-r2);//2
s.addNode(-r2,0,r2);//3
s.addNode(r2/2,r1/2,0);//4
s.addNode(0,r1/2,-r2/2);//5
s.addNode(-r2/2,r1/2,0);//6
s.addNode(0,r1/2,r2/2);//7
}
// Nodes: center points of opposing faces of rectangles
void CraterDeep::addNodes(tgStructure& s) {
const int nBoxes = 4;
// Accumulating rotation on boxes
btVector3 rotationPoint = origin;
btVector3 rotationAxis = btVector3(0, 1, 0); // y-axis
double rotationAngle = M_PI/2;
addBoxNodes();
addBoxNodes();
addBoxNodes();
addBoxNodes();
for(int i=0;i<nodes.size();i+=2) {
s.addNode(nodes[i]);
s.addNode(nodes[i+1]);
s.addRotation(rotationPoint, rotationAxis, rotationAngle);
s.addPair(i, i+1, "box");
}
s.move(btVector3(0, -5, 0)); // Sink boxes into the ground
}
void BigPuppySymmetricArching::addNodesFoot(tgStructure& s, double r1, double r2) {
s.addNode(r2,0,r2);//0
s.addNode(r2,0,-r2);//1
s.addNode(-r2,0,-r2);//2
s.addNode(-r2,0,r2);//3
s.addNode(r2/2,r1/2,0);//4
s.addNode(0,r1/2,-r2/2);//5
s.addNode(-r2/2,r1/2,0);//6
s.addNode(0,r1/2,r2/2);//7
}
void TensegrityModel::removePair(tgStructure& structure, const tgNode* from, const tgNode* to, bool isEdgePair,
const std::vector<tgBuildSpec::RigidAgent*>& rigidAgents, tgBuildSpec& spec) {
tgPair *pair;
try {
pair = &structure.findPair(*from, *to);
} catch (std::invalid_argument e) {
return;
}
for (int i = 0; i < rigidAgents.size(); i++) {
tgTagSearch tagSearch = rigidAgents[i]->tagSearch;
if (tagSearch.matches(*pair)) {
// don't want to remove pairs that are rods
if (isEdgePair) {
throw std::invalid_argument("Edges in node_edge bonds cannot be rods");
}
return;
}
}
structure.removePair(*pair);
}
// Add muscles that connect the segments
void VerticalSpineModel::addMuscles(tgStructure& snake)
{
const std::vector<tgStructure*> children = snake.getChildren();
for (size_t i = 1; i < children.size(); ++i)
{
tgNodes n0 = children[i-1]->getNodes();
tgNodes n1 = children[i ]->getNodes();
// vertical muscles
snake.addPair(n0[0], n1[0], "vertical muscle a");
snake.addPair(n0[1], n1[1], "vertical muscle b");
snake.addPair(n0[2], n1[2], "vertical muscle c");
snake.addPair(n0[3], n1[3], "vertical muscle d");
// saddle muscles
snake.addPair(n0[2], n1[1], tgString("saddle muscle seg", i-1));
snake.addPair(n0[3], n1[1], tgString("saddle muscle seg", i-1));
snake.addPair(n0[2], n1[0], tgString("saddle muscle seg", i-1));
snake.addPair(n0[3], n1[0], tgString("saddle muscle seg", i-1));
}
}
// Nodes: center points of opposing faces of rectangles
void tgCraterDeep::addNodes(tgStructure& s) {
#if (0)
const int nBoxes = 4;
#endif // Suppress compiler warning unused variable
// Accumulating rotation on boxes
btVector3 rotationPoint = origin;
btVector3 rotationAxis = btVector3(0, 1, 0); // y-axis
double rotationAngle = M_PI/2;
addBoxNodes();
addBoxNodes();
addBoxNodes();
addBoxNodes();
for(std::size_t i=0;i<nodes.size();i+=2) {
s.addNode(nodes[i]);
s.addNode(nodes[i+1]);
s.addRotation(rotationPoint, rotationAxis, rotationAngle);
s.addPair(i, i+1, "box");
}
s.move(btVector3(0, -5, 0)); // Sink boxes into the ground
}
void TensegrityModel::addChild(tgStructure& structure, const std::string &parentPath,
const std::string& childName, const Yam& childStructurePath, tgBuildSpec& spec) {
if (!childStructurePath) return;
std::string childPath = childStructurePath.as<std::string>();
// if path is relative, use path relative to parent structure
if (childPath[0] != '/') {
childPath = parentPath.substr(0, parentPath.rfind("/") + 1) + childPath;
}
tgStructure childStructure = tgStructure(childName);
buildStructure(childStructure, childPath, spec);
structure.addChild(childStructure);
}
void VerticalSpineModel::addSegments(tgStructure& snake, const tgStructure& tetra,
double edge, size_t segmentCount)
{
//const btVector3 offset(0, 0, -edge * 1.15);
const btVector3 offset(0, 7.5, 0);
for (size_t i = 1; i < segmentCount; ++i)
{
tgStructure* const t = new tgStructure(tetra);
t->addTags(tgString("segment", i + 1));
t->move((i + 1) * offset);
// Add a child to the snake
snake.addChild(t);
}
}
// Add muscles that connect the segments
void DuCTTTestModel::addMuscles(tgStructure& snake)
{
const std::vector<tgStructure*> children = snake.getChildren();
for (size_t i = 1; i < children.size(); ++i)
{
tgNodes n0 = children[i-1]->getNodes();
tgNodes n1 = children[i ]->getNodes();
snake.addPair(n0[0], n1[0], "top right muscle");
snake.addPair(n0[1], n1[1], "top left muscle");
snake.addPair(n0[2], n1[0], "front right muscle");
snake.addPair(n0[2], n1[1], "front left muscle");
snake.addPair(n0[3], n1[0], "back right muscle");
snake.addPair(n0[3], n1[1], "back left muscle");
snake.addPair(n0[2], n1[2], "bottom front muscle");
snake.addPair(n0[3], n1[3], "bottom back muscle");
}
}
