Skeleton * BVHParser::createSkeleton()
{
Skeleton * s = new Skeleton();
// set default pose...
Joint * b = createJoint(_root);
if( !s->setJoints(b) )
{
delete s;
return 0;
}
Pose * pose = new Pose(s->getNumJoints());
for(int i = 0; i < _linearNodes.size(); i++ )
{
BVHNode * n = _linearNodes[i];
pose->transforms[i].rotation.identity();
pose->transforms[i].position(0,0,0);
}
s->pose = pose;
s->init();
return s;
}
void Skeleton::copyFrom( Skeleton* source_skeleton )
{
Joint* source_root = source_skeleton->getRootJoint();
if( !source_root )
{
return;
}
this->cleanup();
std::vector< Joint* >* source_joints = source_skeleton->getJointList();
std::vector< Joint* >::iterator itor_j = source_joints->begin();
while( itor_j != source_joints->end() )
{
Joint* source_joint = ( *itor_j ++ );
Joint* source_parent = source_joint->getParent();
std::string name = source_joint->getName();
math::vector offset = source_joint->getOffset();
unsigned int human_id = source_joint->getHumanID();
Joint* this_parent = 0;
if( source_parent )
{
unsigned int parent_id = source_parent->getIndex();
this_parent = joint_list[ parent_id ];
}
Joint* this_joint = createJoint( this_parent, name );
this_joint->setOffset( offset );
setHumanJoint( name, human_id );
}
}
void Joint::build(const Vector3d &v, const Rotation &r, bool dynamics)
{
if (m_bodyNum != BODY_NUM) { return; }
assert(m_world);
m_joint = createJoint(m_bodies[0], m_bodies[1]);
Parts *parts = (Parts*) dBodyGetData(m_bodies[1]);
assert(parts);
double x, y, z;
parts->givePosition(x, y, z);
m_rotv.set(x, y, z);
m_rotv -= m_anchor;
Vector3d av = m_anchor;
av.rotate(r);
av += v;
applyAnchor(av.x(), av.y(), av.z());
if (m_fixed) {
dJointSetFixed(m_joint);
}
if (dynamics) {
m_jfb = new dJointFeedback;
dJointSetFeedback(m_joint, m_jfb);
}
}
void Ski::create(b2World * b2dworld, float x, float y, float w, float h) {
b2Vec2 center = screenPtToWorldPt(ofVec2f(x,y));
skiWidth = w;
skiHeight = h;
start = false;
takeoff = false;
blow = false;
wind = 0;
// Define the body and make it from the shape
b2BodyDef bd;
bd.type = b2_dynamicBody;
bd.position.Set(center.x, center.y);
b2Body* body = b2dworld->CreateBody(&bd);
// Define a polygon (this is what we use for a rectangle)
b2PolygonShape ps;
ps.SetAsBox(b2dNum(w/2), b2dNum(h/2));
// Define a fixture
b2FixtureDef fd;
fd.shape = &ps;
// Parameters that affect physics
fd.density = 20;
fd.friction = 0.001;
fd.restitution = 0;
// attach fixture to body
body->CreateFixture(&fd);
// Give it some initial random velocity
//body.setLinearVelocity(new Vec2(100,0));
body->SetAngularVelocity(0);
bodies.push_back(body);
b2Vec2 frontPos = center;
frontPos.x += b2dNum(skiWidth/2);
createWheel(b2dworld, frontPos, skiHeight/2);
b2Vec2 backPos = center;
backPos.x -= b2dNum(skiWidth/2);
createWheel(b2dworld, backPos, skiHeight/2);
createJoint(b2dworld, bodies.at(0), bodies.at(1));
createJoint(b2dworld, bodies.at(0), bodies.at(2));
}
DistanceJoint::DistanceJoint(Body * body1, Body * body2, float x1, float y1, float x2, float y2, bool collideConnected)
: Joint(body1, body2), joint(NULL)
{
b2DistanceJointDef def;
def.Initialize(body1->body, body2->body, Physics::scaleDown(b2Vec2(x1,y1)), Physics::scaleDown(b2Vec2(x2,y2)));
def.collideConnected = collideConnected;
joint = (b2DistanceJoint*)createJoint(&def);
}
RevoluteJoint::RevoluteJoint(Body *body1, Body *body2, float xA, float yA, float xB, float yB, bool collideConnected)
: Joint(body1, body2)
, joint(NULL)
{
b2RevoluteJointDef def;
init(def, body1, body2, xA, yA, xB, yB, collideConnected);
joint = (b2RevoluteJoint *)createJoint(&def);
}
FrictionJoint::FrictionJoint(Body * body1, Body * body2, float xA, float yA, float xB, float yB, bool collideConnected)
: Joint(body1, body2), joint(NULL)
{
b2FrictionJointDef def;
def.Initialize(body1->body, body2->body, Physics::scaleDown(b2Vec2(xA,yA)));
def.localAnchorB = body2->body->GetLocalPoint(Physics::scaleDown(b2Vec2(xB, yB)));
def.collideConnected = collideConnected;
joint = (b2FrictionJoint*)createJoint(&def);
}
PulleyJoint::PulleyJoint(Body * body1, Body * body2, b2Vec2 groundAnchor1, b2Vec2 groundAnchor2, b2Vec2 anchor1, b2Vec2 anchor2, float ratio)
: Joint(body1, body2), joint(NULL)
{
b2PulleyJointDef def;
def.Initialize(body1->body, body2->body, world->scaleDown(groundAnchor1), world->scaleDown(groundAnchor2), \
world->scaleDown(anchor1), world->scaleDown(anchor2), ratio);
joint = (b2PulleyJoint*)createJoint(&def);
}
PrismaticJoint::PrismaticJoint(boost::shared_ptr<Body> body1, boost::shared_ptr<Body> body2, b2PrismaticJointDef * def)
: Joint(body1, body2)
{
def->Initialize(body1->body, body2->body, def->localAnchor2, def->localAxis1);
def->lowerTranslation = 0.0f;
def->upperTranslation = 100.0f;
def->enableLimit = true;
joint = (b2PrismaticJoint*)createJoint(def);
}
RevoluteJoint::RevoluteJoint(Body *body1, Body *body2, float x, float y, bool collideConnected)
: Joint(body1, body2)
, joint(NULL)
{
b2RevoluteJointDef def;
def.Initialize(body1->body, body2->body, Physics::scaleDown(b2Vec2(x,y)));
def.collideConnected = collideConnected;
joint = (b2RevoluteJoint *)createJoint(&def);
}
void EffortDisplay::load()
{
// get robot_description
std::string content;
if (!update_nh_.getParam( robot_description_property_->getStdString(), content) ) {
std::string loc;
if( update_nh_.searchParam( robot_description_property_->getStdString(), loc ))
{
update_nh_.getParam( loc, content );
}
else
{
clear();
setStatus( rviz::StatusProperty::Error, "URDF",
"Parameter [" + robot_description_property_->getString()
+ "] does not exist, and was not found by searchParam()" );
return;
}
}
if( content.empty() )
{
clear();
setStatus( rviz::StatusProperty::Error, "URDF", "URDF is empty" );
return;
}
if( content == robot_description_ )
{
return;
}
robot_description_ = content;
robot_model_ = boost::shared_ptr<urdf::Model>(new urdf::Model());
if (!robot_model_->initString(content))
{
ROS_ERROR("Unable to parse URDF description!");
setStatus( rviz::StatusProperty::Error, "URDF", "Unable to parse robot model description!");
return;
}
setStatus(rviz::StatusProperty::Ok, "URDF", "Robot model parserd Ok");
for (std::map<std::string, boost::shared_ptr<urdf::Joint> >::iterator it = robot_model_->joints_.begin(); it != robot_model_->joints_.end(); it ++ ) {
boost::shared_ptr<urdf::Joint> joint = it->second;
if ( joint->type == urdf::Joint::REVOLUTE ) {
std::string joint_name = it->first;
boost::shared_ptr<urdf::JointLimits> limit = joint->limits;
joints_[joint_name] = createJoint(joint_name);
//joints_[joint_name]->max_effort_property_->setFloat(limit->effort);
//joints_[joint_name]->max_effort_property_->setReadOnly( true );
joints_[joint_name]->setMaxEffort(limit->effort);
}
}
}
PulleyJoint::PulleyJoint(Body *bodyA, Body *bodyB, b2Vec2 groundAnchorA, b2Vec2 groundAnchorB, b2Vec2 anchorA, b2Vec2 anchorB, float ratio, bool collideConnected)
: Joint(bodyA, bodyB)
, joint(NULL)
{
b2PulleyJointDef def;
def.Initialize(bodyA->body, bodyB->body, Physics::scaleDown(groundAnchorA), Physics::scaleDown(groundAnchorB), \
Physics::scaleDown(anchorA), Physics::scaleDown(anchorB), ratio);
def.collideConnected = collideConnected;
joint = (b2PulleyJoint *)createJoint(&def);
}
RopeJoint::RopeJoint(Body * body1, Body * body2, float x1, float y1, float x2, float y2, float maxLength, bool collideConnected)
: Joint(body1, body2), joint(NULL)
{
b2RopeJointDef def;
def.bodyA = body1->body;
def.bodyB = body2->body;
body1->getLocalPoint(Physics::scaleDown(x1), Physics::scaleDown(y1), def.localAnchorA.x, def.localAnchorA.y);
body2->getLocalPoint(Physics::scaleDown(x2), Physics::scaleDown(y2), def.localAnchorB.x, def.localAnchorB.y);
def.maxLength = Physics::scaleDown(maxLength);
def.collideConnected = collideConnected;
joint = (b2RopeJoint*)createJoint(&def);
}
MouseJoint::MouseJoint(Body *body1, float x, float y)
: Joint(body1)
, joint(NULL)
{
b2MouseJointDef def;
def.bodyA = body1->world->getGroundBody();
def.bodyB = body1->body;
def.maxForce = 1000.0f * body1->body->GetMass();
def.target = Physics::scaleDown(b2Vec2(x,y));
joint = (b2MouseJoint *)createJoint(&def);
}
GearJoint::GearJoint(Joint * joint1, Joint * joint2, float ratio)
: Joint(joint1->body2, joint2->body2)
{
b2GearJointDef def;
def.joint1 = joint1->joint;
def.joint2 = joint2->joint;
def.body1 = joint1->body2->body;
def.body2 = joint2->body2->body;
def.ratio = ratio;
joint = (b2GearJoint*)createJoint(&def);
}
DrawingWindow::DrawingWindow(QVector<QPointF> *customRulePoints, QWidget *parent) : QGraphicsView(parent)
{
//Connect main window with this one by custome rule points vector
this->customRulePoints = customRulePoints;
//Create drawing scene
drawingScene = new QGraphicsScene;
drawingScene->setSceneRect(0,0,1000,1000);
setScene(drawingScene);
//Configure button that create new joint of custom rule
createJointButton = new QPushButton("New joint", this);
connect(createJointButton,SIGNAL(clicked()),
this,SLOT(createJoint()));
//Configure button that delete last created joint of custom rule
deleteJointButton = new QPushButton("Delete joint", this);
connect(deleteJointButton,SIGNAL(clicked()),
this,SLOT(deleteJoint()));
//Configure button that accepts and save custom rule
acceptCustomRule = new QPushButton("Accept", this);
connect(acceptCustomRule,SIGNAL(clicked()),
this,SLOT(acceptRule()));
//Configure button that cancel and restore previous custom rule
cancelCustomRule = new QPushButton("Cancel", this);
connect(cancelCustomRule,SIGNAL(clicked()),
this,SLOT(cancelRule()));
//Place all buttons in one widget with vertical layout
drawingSceneWidget = new QWidget;
drawingSceneWidget->resize(200, 200);
drawingSceneWidget->setLayout(&drawingSceneLayout);
drawingSceneLayout.addWidget(createJointButton);
drawingSceneLayout.addWidget(deleteJointButton);
drawingSceneLayout.addWidget(acceptCustomRule);
drawingSceneLayout.addWidget(cancelCustomRule);
drawingScene->addWidget(drawingSceneWidget);
//Make scene dynamically redraw
connect(drawingScene,SIGNAL(changed(QList<QRectF>)),
this,SLOT(redraw()));
//Draw line showing custom rule
customRule = new QGraphicsPathItem();
customRule->setPath(createPath(joints));
drawingScene->addItem(customRule);
//Make it draws smooth and resize
setRenderHint(QPainter::Antialiasing);
resize(1000,1000);
}
GearJoint::GearJoint(Joint *joint1, Joint *joint2, float ratio, bool collideConnected)
: Joint(joint1->body2, joint2->body2)
, joint(NULL)
{
b2GearJointDef def;
def.joint1 = joint1->joint;
def.joint2 = joint2->joint;
def.bodyA = joint1->body2->body;
def.bodyB = joint2->body2->body;
def.ratio = ratio;
def.collideConnected = collideConnected;
joint = (b2GearJoint *)createJoint(&def);
}
Joint * BVHParser::createJoint( BVHNode * node )
{
Joint * b = new Joint();
b->name = node->name;
b->offset = node->offset;
for( int i = 0; i < node->children.size(); i++ )
{
if(!node->children[i]->endNode)
b->addChild( createJoint( node->children[i] ) );
}
return b;
}
void Physics::testPhysics(){
int box3 = createBox(895,95,395);
int box4 = createBox(195,195,195);
createJoint(box3, box4,0, 50, 50, 2, 50, 50,50, 30,30,30);
/*
int box = createBox(85,385,185);
createJoint(box3, box, 0,50, 50, 3, 50, 50, 3, 0,0,0);
int box5 = createBox(95,95,395);
createJoint(box, box5, 0,50, 50,5, 50, 50, 0, 0,0,0);
*/
// createSensor(box2, pressure);
//NN test
std::vector<NeuralNode*> inputs;
for(int i=0;i< (int) sensors.size();i++){
inputs.push_back(new NeuralNode(&sensors.at(i)));
}
inputs.push_back(new NeuralNode(1)); //index 3
inputs.push_back(new NeuralNode(-2)); //index 4
float* testPoint = new float;
*testPoint = 5;
inputs.push_back(new NeuralNode(testPoint)); //index 5
theNet = new NeuralNetwork(inputs);
theNet->insertNode(SUM,5,3,3,1);
theNet->insertNode(SIN,1,1);
theNet->changeLayer();
theNet->insertNode(PRODUCT,0,1,2,2);
theNet->stopBuilding();
NeuralNetwork* aNet = new NeuralNetwork(theNet->getLastLayer());
aNet->insertNode(PRODUCT,0,0,10000,10000);
aNet->stopBuilding();
subnets.push_back(aNet);
effectorNNindex.push_back(0);
effectorNNindex.push_back(1);
effectorNNindex.push_back(2);
solveGroundConflicts();
}
/*! Creates a new joint owned by this skeleton with a default name.
*/
SLJoint* SLSkeleton::createJoint(SLuint id)
{
ostringstream oss;
oss << "Joint " << id;
return createJoint(oss.str(), id);
}
void ClockCircle :: createJoint ( const ofPoint& p )
{
createJoint( p.x, p.y );
}
void ClockCircle :: createLineupJoint ()
{
createJoint( lineUpPoint.x, lineUpPoint.y, 0 );
bLineupJoint = true;
}
void ClockCircle :: createOuterJoint ()
{
createJoint( 0.5, 0.5, screenHeight * 0.38 );
bOuterJoint = true;
}
void ClockCircle :: createCenterJoint ()
{
createJoint( 0.5, 0.5, ofRandom( 0, screenHeight * 0.2 ) );
bCenterJoint = true;
}
KeplerCube::KeplerCube(btDynamicsWorld* ownerWorld, const btVector3& positionOffset)
: m_ownerWorld (ownerWorld)
{
// const int R = 3.0f;
const int L = R_LENGTH/2.0f;
//
// const float d = L/2.0f;
// for (int i=0; i<N_EDGES; i++)
// m_shapes.push_back(new btBoxShape(btVector3());
// Setup all the rigid bodies
btTransform offset; offset.setIdentity();
offset.setOrigin(positionOffset);
for (int i=0; i<N_EDGES; i++) {
btTransform transform = createBoxTransform(i);
m_bodies.push_back(localCreateRigidBody(btScalar(1.0f), offset*transform, m_shapes.at(i)));
}
// Setup some damping on the m_bodies
for (int i = 0; i < m_bodies.size(); ++i) {
// m_bodies.at(i)->setDamping(0.05, 0.85);
// m_bodies.at(i)->setDeactivationTime(0.8);
// m_bodies.at(i)->setSleepingThresholds(1.6, 2.5);
// m_bodies.at(i)->setActivationState(DISABLE_DEACTIVATION);
// m_bodies.at(i)->setCollisionFlags(m_bodies.at(i)->getCollisionFlags() |
// btCollisionObject::CF_KINEMATIC_OBJECT);
}
createJoint(m_bodies.at(0), m_bodies.at(1), btVector3( 0, 0,-L), btVector3(-L, 0, 0), 0, 0, 0, 0, 0, 0);
createJoint(m_bodies.at(1), m_bodies.at(2), btVector3( L, 0, 0), btVector3( 0, 0,-L), 0, 0, 0, 0, 0, 0);
createJoint(m_bodies.at(2), m_bodies.at(3), btVector3( 0, 0, L), btVector3( L, 0, 0), 0, 0, 0, 0, 0, 0);
createJoint(m_bodies.at(3), m_bodies.at(0), btVector3(-L, 0, 0), btVector3( 0, 0, L), 0, 0, 0, 0, 0, 0);
createJoint(m_bodies.at(4), m_bodies.at(0), btVector3( 0,-L, 0), btVector3( 0, 0,-L), 0, 0, 0, 0, 0, 0);
createJoint(m_bodies.at(5), m_bodies.at(1), btVector3( 0,-L, 0), btVector3( L, 0, 0), 0, 0, 0, 0, 0, 0);
createJoint(m_bodies.at(6), m_bodies.at(2), btVector3( 0,-L, 0), btVector3( 0, 0, L), 0, 0, 0, 0, 0, 0);
createJoint(m_bodies.at(7), m_bodies.at(3), btVector3( 0,-L, 0), btVector3(-L, 0, 0), 0, 0, 0, 0, 0, 0);
createJoint(m_bodies.at(4), m_bodies.at( 8), btVector3( 0, L, 0), btVector3( 0, 0,-L), 0, 0, 0, 0, 0, 0);
createJoint(m_bodies.at(5), m_bodies.at( 9), btVector3( 0, L, 0), btVector3( L, 0, 0), 0, 0, 0, 0, 0, 0);
createJoint(m_bodies.at(6), m_bodies.at(10), btVector3( 0, L, 0), btVector3( 0, 0, L), 0, 0, 0, 0, 0, 0);
createJoint(m_bodies.at(7), m_bodies.at(11), btVector3( 0, L, 0), btVector3(-L, 0, 0), 0, 0, 0, 0, 0, 0);
createJoint(m_bodies.at( 8), m_bodies.at( 9), btVector3( 0, 0,-L), btVector3(-L, 0, 0), 0, 0, 0, 0, 0, 0);
createJoint(m_bodies.at( 9), m_bodies.at(10), btVector3( L, 0, 0), btVector3( 0, 0,-L), 0, 0, 0, 0, 0, 0);
createJoint(m_bodies.at(10), m_bodies.at(11), btVector3( 0, 0, L), btVector3( L, 0, 0), 0, 0, 0, 0, 0, 0);
createJoint(m_bodies.at(11), m_bodies.at( 8), btVector3(-L, 0, 0), btVector3( 0, 0, L), 0, 0, 0, 0, 0, 0);
}
Gears::Gears(const b2Vec2 &gravity, QObject *parent) :
QBox2DTest(gravity, parent)
{
//modified codes taken from Gears example
b2RevoluteJoint* m_joint1;
b2RevoluteJoint* m_joint2;
b2PrismaticJoint* m_joint3;
b2GearJoint* m_joint4;
b2GearJoint* m_joint5;
QBox2DBody* ground = NULL;
{
b2BodyDef bd;
ground = createBody(&bd);
b2EdgeShape shape;
shape.Set(b2Vec2(50.0f, 0.0f), b2Vec2(-50.0f, 0.0f));
ground->createFixture(&shape, 0.0f);
}
// Gears co
{
b2CircleShape circle1;
circle1.m_radius = 1.0f;
b2PolygonShape box;
box.SetAsBox(0.5f, 5.0f);
b2CircleShape circle2;
circle2.m_radius = 2.0f;
b2BodyDef bd1;
bd1.type = b2_staticBody;
bd1.position.Set(10.0f, 9.0f);
QBox2DBody* body1 = createBody(&bd1);
body1->createFixture(&circle1, 0.0f);
b2BodyDef bd2;
bd2.type = b2_dynamicBody;
bd2.position.Set(10.0f, 8.0f);
QBox2DBody* body2 = createBody(&bd2);
body2->createFixture(&box, 5.0f);
b2BodyDef bd3;
bd3.type = b2_dynamicBody;
bd3.position.Set(10.0f, 6.0f);
QBox2DBody* body3 = createBody(&bd3);
body3->createFixture(&circle2, 5.0f);
b2RevoluteJointDef jd1;
jd1.Initialize(body2->body(), body1->body(), bd1.position);
QBox2DJoint* joint1 = createJoint(&jd1);
b2RevoluteJointDef jd2;
jd2.Initialize(body2->body(), body3->body(), bd3.position);
QBox2DJoint* joint2 = createJoint(&jd2);
b2GearJointDef jd4;
jd4.bodyA = body1->body();
jd4.bodyB = body3->body();
jd4.joint1 = joint1->joint();
jd4.joint2 = joint2->joint();
jd4.ratio = circle2.m_radius / circle1.m_radius;
createJoint(&jd4);
}
{
b2CircleShape circle1;
circle1.m_radius = 1.0f;
b2CircleShape circle2;
circle2.m_radius = 2.0f;
b2PolygonShape box;
box.SetAsBox(0.5f, 5.0f);
b2BodyDef bd1;
bd1.type = b2_dynamicBody;
bd1.position.Set(-3.0f, 12.0f);
QBox2DBody* body1 = createBody(&bd1);
body1->createFixture(&circle1, 5.0f);
b2RevoluteJointDef jd1;
jd1.bodyA = ground->body();
jd1.bodyB = body1->body();
jd1.localAnchorA = ground->getLocalPoint(bd1.position);
jd1.localAnchorB = body1->getLocalPoint(bd1.position);
jd1.referenceAngle = body1->getAngle() - ground->getAngle();
m_joint1 = (b2RevoluteJoint*)createJoint(&jd1)->joint();
b2BodyDef bd2;
bd2.type = b2_dynamicBody;
bd2.position.Set(0.0f, 12.0f);
QBox2DBody* body2 = createBody(&bd2);
body2->createFixture(&circle2, 5.0f);
b2RevoluteJointDef jd2;
jd2.Initialize(ground->body(), body2->body(), bd2.position);
m_joint2 = (b2RevoluteJoint*)createJoint(&jd2)->joint();
b2BodyDef bd3;
bd3.type = b2_dynamicBody;
bd3.position.Set(2.5f, 12.0f);
QBox2DBody* body3 = createBody(&bd3);
body3->createFixture(&box, 5.0f);
b2PrismaticJointDef jd3;
jd3.Initialize(ground->body(), body3->body(), bd3.position, b2Vec2(0.0f, 1.0f));
jd3.lowerTranslation = -5.0f;
jd3.upperTranslation = 5.0f;
jd3.enableLimit = true;
m_joint3 = (b2PrismaticJoint*)createJoint(&jd3)->joint();
b2GearJointDef jd4;
jd4.bodyA = body1->body();
jd4.bodyB = body2->body();
jd4.joint1 = m_joint1;
jd4.joint2 = m_joint2;
jd4.ratio = circle2.m_radius / circle1.m_radius;
m_joint4 = (b2GearJoint*)createJoint(&jd4)->joint();
b2GearJointDef jd5;
jd5.bodyA = body2->body();
jd5.bodyB = body3->body();
jd5.joint1 = m_joint2;
jd5.joint2 = m_joint3;
jd5.ratio = -1.0f / circle2.m_radius;
m_joint5 = (b2GearJoint*)createJoint(&jd5)->joint();
}
}
DistanceJoint::DistanceJoint(boost::shared_ptr<Body> body1, boost::shared_ptr<Body> body2, b2DistanceJointDef * def)
: Joint(body1, body2)
{
def->Initialize(body1->body, body2->body, def->localAnchor1, def->localAnchor2);
joint = (b2DistanceJoint*)createJoint(def);
}
Web::Web(const b2Vec2 &gravity, QObject *parent) :
QBox2DTest(gravity, parent)
{
QBox2DBody* ground = NULL;
{
b2BodyDef bd;
ground = createBody(&bd);
b2EdgeShape shape;
shape.Set(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f));
ground->createFixture(&shape, 0.0f);
}
{
b2PolygonShape shape;
shape.SetAsBox(0.5f, 0.5f);
b2BodyDef bd;
bd.type = b2_dynamicBody;
bd.position.Set(-5.0f, 5.0f);
m_bodies[0] = createBody(&bd);
m_bodies[0]->createFixture(&shape, 5.0f);
bd.position.Set(5.0f, 5.0f);
m_bodies[1] = createBody(&bd);
m_bodies[1]->createFixture(&shape, 5.0f);
bd.position.Set(5.0f, 15.0f);
m_bodies[2] = createBody(&bd);
m_bodies[2]->createFixture(&shape, 5.0f);
bd.position.Set(-5.0f, 15.0f);
m_bodies[3] = createBody(&bd);
m_bodies[3]->createFixture(&shape, 5.0f);
b2DistanceJointDef jd;
b2Vec2 p1, p2, d;
jd.frequencyHz = 2.0f;
jd.dampingRatio = 0.0f;
jd.bodyA = ground->body();
jd.bodyB = m_bodies[0]->body();
jd.localAnchorA.Set(-10.0f, 0.0f);
jd.localAnchorB.Set(-0.5f, -0.5f);
p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA);
p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB);
d = p2 - p1;
jd.length = d.Length();
m_joints[0] = createJoint(&jd);
jd.bodyA = ground->body();
jd.bodyB = m_bodies[1]->body();
jd.localAnchorA.Set(10.0f, 0.0f);
jd.localAnchorB.Set(0.5f, -0.5f);
p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA);
p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB);
d = p2 - p1;
jd.length = d.Length();
m_joints[1] = createJoint(&jd);
jd.bodyA = ground->body();
jd.bodyB = m_bodies[2]->body();
jd.localAnchorA.Set(10.0f, 20.0f);
jd.localAnchorB.Set(0.5f, 0.5f);
p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA);
p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB);
d = p2 - p1;
jd.length = d.Length();
m_joints[2] = createJoint(&jd);
jd.bodyA = ground->body();
jd.bodyB = m_bodies[3]->body();
jd.localAnchorA.Set(-10.0f, 20.0f);
jd.localAnchorB.Set(-0.5f, 0.5f);
p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA);
p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB);
d = p2 - p1;
jd.length = d.Length();
m_joints[3] = createJoint(&jd);
jd.bodyA = m_bodies[0]->body();
jd.bodyB = m_bodies[1]->body();
jd.localAnchorA.Set(0.5f, 0.0f);
jd.localAnchorB.Set(-0.5f, 0.0f);;
p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA);
p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB);
d = p2 - p1;
jd.length = d.Length();
m_joints[4] = createJoint(&jd);
jd.bodyA = m_bodies[1]->body();
jd.bodyB = m_bodies[2]->body();
jd.localAnchorA.Set(0.0f, 0.5f);
jd.localAnchorB.Set(0.0f, -0.5f);
p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA);
p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB);
d = p2 - p1;
jd.length = d.Length();
m_joints[5] = createJoint(&jd);
jd.bodyA = m_bodies[2]->body();
jd.bodyB = m_bodies[3]->body();
jd.localAnchorA.Set(-0.5f, 0.0f);
jd.localAnchorB.Set(0.5f, 0.0f);
p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA);
p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB);
d = p2 - p1;
jd.length = d.Length();
m_joints[6] = createJoint(&jd);
jd.bodyA = m_bodies[3]->body();
jd.bodyB = m_bodies[0]->body();
jd.localAnchorA.Set(0.0f, -0.5f);
jd.localAnchorB.Set(0.0f, 0.5f);
p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA);
p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB);
d = p2 - p1;
jd.length = d.Length();
m_joints[7] = createJoint(&jd);
}
}