/* * Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages * arising from the use of this software. * Permission is granted to anyone to use this software for any purpose, * including commercial applications, and to alter it and redistribute it * freely, subject to the following restrictions: * 1. The origin of this software must not be misrepresented; you must not * claim that you wrote the original software. If you use this software * in a product, an acknowledgment in the product documentation would be * appreciated but is not required. * 2. Altered source versions must be plainly marked as such, and must not be * misrepresented as being the original software. * 3. This notice may not be removed or altered from any source distribution. */ #ifndef ELASTIC_BODY_H #define ELASTIC_BODY_H class ElasticBody : public Test { public: b2Body* bodies[64]; b2Body* m_ground; b2Body* m_elev; b2PrismaticJoint* m_joint_elev; /// Main... ElasticBody() { /// Bottom static body { b2PolygonDef sd; sd.SetAsBox(50.0f, 2.0f); sd.friction = 0.1f; sd.restitution = 0.1f; b2BodyDef bd; bd.position.Set(-1.0f, -7.5f); m_ground = m_world->CreateBody(&bd); m_ground->CreateFixture(&sd); } /// Upper static body { b2PolygonDef sd; sd.SetAsBox(20.0f, 0.50f,b2Vec2(0.f,0.f),0.047f*b2_pi); sd.friction = 0.01f; sd.restitution = 0.001f; b2BodyDef bd; bd.position.Set(-20.f, 93.0f); b2Body* g = m_world->CreateBody(&bd); g->CreateFixture(&sd); sd.SetAsBox(15.f, 0.50f,b2Vec2(-15.0f,12.5f),0.0f); g->CreateFixture(&sd); sd.SetAsBox(20.f,0.5f,b2Vec2(0.0f,-25.0f),-0.5f); g->CreateFixture(&sd); } /// Left channel left wall { b2PolygonDef sd; sd.SetAsBox(0.7f, 55.0f); sd.friction = 0.1f; sd.restitution = 0.1f; b2BodyDef bd; bd.position.Set(-49.3f, 50.0f); b2Body* g = m_world->CreateBody(&bd); g->CreateFixture(&sd); } /// Right wall { b2PolygonDef sd; sd.SetAsBox(0.7f, 55.0f); sd.friction = 0.1f; sd.restitution = 0.1f; b2BodyDef bd; bd.position.Set(45.f, 50.0f); b2Body* g = m_world->CreateBody(&bd); g->CreateFixture(&sd); } /// Left channel right upper wall { b2PolygonDef sd; sd.SetAsBox(0.5f, 20.0f); sd.friction = 0.05f; sd.restitution = 0.01f; b2BodyDef bd; bd.position.Set(-42.0f, 70.0f); bd.angle = -0.03f*b2_pi; b2Body* g = m_world->CreateBody(&bd); g->CreateFixture(&sd); } /// Left channel right lower wall { b2PolygonDef sd; sd.SetAsBox(0.50f, 23.0f); sd.friction = 0.05f; sd.restitution = 0.01f; b2BodyDef bd; bd.position.Set(-44.0f, 27.0f); b2Body* g = m_world->CreateBody(&bd); g->CreateFixture(&sd); /// Bottom motors b2CircleDef cd; cd.radius = 3.0f; cd.density = 15.0f; cd.friction = 1.f; cd.restitution = 0.2f; /// 1. bd.position.Set(-40.0f,2.5f); b2Body* body = m_world->CreateBody(&bd); body->CreateFixture(&cd); body->SetMassFromShapes(); b2RevoluteJointDef jr; jr.Initialize (g,body,body->GetWorldCenter()+b2Vec2(0.f,1.f)); jr.maxMotorTorque = 30000.f; jr.enableMotor = true; jr.motorSpeed = 20.f; m_world->CreateJoint(&jr); /// 1. left down bd.position.Set(-46.0f,-2.5f); cd. radius = 1.5f; jr.motorSpeed = -20.f; body = m_world->CreateBody(&bd); body->CreateFixture(&cd); sd.SetAsBox(2.0f, 0.50f); body->CreateFixture(&sd); body->SetMassFromShapes(); jr.Initialize (g,body,body->GetWorldCenter()); m_world->CreateJoint(&jr); /// 2. cd.radius = 3.0f; jr.motorSpeed = 20.f; bd.position.Set(-32.0f,2.5f); body = m_world->CreateBody(&bd); body->CreateFixture(&cd); body->SetMassFromShapes(); jr.Initialize (g,body,body->GetWorldCenter()+b2Vec2(0.f,1.f)); m_world->CreateJoint(&jr); /// 3. jr.motorSpeed = 20.f; bd.position.Set(-24.0f,1.5f); body = m_world->CreateBody(&bd); body->CreateFixture(&cd); body->SetMassFromShapes(); jr.Initialize (g,body,body->GetWorldCenter()+b2Vec2(0.f,1.f)); m_world->CreateJoint(&jr); /// 4. bd.position.Set(-16.0f,0.8f); body = m_world->CreateBody(&bd); body->CreateFixture(&cd); body->SetMassFromShapes(); jr.Initialize (g,body,body->GetWorldCenter()+b2Vec2(0.f,1.f)); m_world->CreateJoint(&jr); /// 5. bd.position.Set(-8.0f,0.5f); body = m_world->CreateBody(&bd); body->CreateFixture(&cd); body->SetMassFromShapes(); jr.Initialize (g,body,body->GetWorldCenter()+b2Vec2(0.f,1.f)); m_world->CreateJoint(&jr); /// 6. bd.position.Set(0.0f,0.1f); body = m_world->CreateBody(&bd); body->CreateFixture(&cd); body->SetMassFromShapes(); jr.Initialize (g,body,body->GetWorldCenter()+b2Vec2(0.f,1.f)); m_world->CreateJoint(&jr); /// 7. bd.position.Set(8.0f,-0.5f); body = m_world->CreateBody(&bd); body->CreateFixture(&cd); sd.SetAsBox(3.7f, 0.5f); body->CreateFixture(&sd); body->SetMassFromShapes(); jr.Initialize (g,body,body->GetWorldCenter()+b2Vec2(0.f,1.f)); m_world->CreateJoint(&jr); /// 8. right rotator sd.SetAsBox(5.f, 0.5f); sd.density = 2.0f; bd.position.Set(18.0f,1.f); b2Body* rightmotor = m_world->CreateBody(&bd); rightmotor->CreateFixture(&sd); sd.SetAsBox(4.5f, 0.5f, b2Vec2(0.f,0.f),b2_pi/3.f); rightmotor->CreateFixture(&sd); sd.SetAsBox(4.5f, 0.5f, b2Vec2(0.f,0.f),b2_pi*2.f/3.f); rightmotor->CreateFixture(&sd); cd.radius = 4.2f; rightmotor->CreateFixture(&cd); rightmotor->SetMassFromShapes(); jr.Initialize (g,rightmotor,rightmotor->GetWorldCenter()); jr.maxMotorTorque = 70000.f; jr.motorSpeed = -4.f; m_world->CreateJoint(&jr); /// 9. left rotator sd.SetAsBox(8.5f, 0.5f); sd.density = 2.0f; bd.position.Set(-34.0f,17.f); body = m_world->CreateBody(&bd); body->CreateFixture(&sd); sd.SetAsBox(8.5f, 0.5f, b2Vec2(0.f,0.f),b2_pi*.5f); body->CreateFixture(&sd); cd.radius = 7.f; cd.friction = 0.9f; body->CreateFixture(&cd); body->SetMassFromShapes(); jr.Initialize (g,body,body->GetWorldCenter()); jr.maxMotorTorque = 100000.f; jr.motorSpeed = -5.f; m_world->CreateJoint(&jr); /// big compressor sd.SetAsBox(3.0f,4.f); sd.density = 10.0f; bd.position.Set(-16.0f,17.f); b2Body *hammerleft = m_world->CreateBody(&bd); hammerleft->CreateFixture(&sd); hammerleft->SetMassFromShapes(); b2DistanceJointDef jd; jd.Initialize(body, hammerleft, body->GetWorldCenter()+b2Vec2(0.f,6.f), hammerleft->GetWorldCenter() ); m_world->CreateJoint(&jd); bd.position.Set(4.0f,17.f); b2Body *hammerright = m_world->CreateBody(&bd); hammerright->CreateFixture(&sd); hammerright->SetMassFromShapes(); jd.Initialize(body, hammerright, body->GetWorldCenter()-b2Vec2(0.f,6.f), hammerright->GetWorldCenter() ); m_world->CreateJoint(&jd); /// pusher sd.SetAsBox(6.f,0.75f); bd.position.Set(-21.0f,9.f); b2Body* pusher = m_world->CreateBody(&bd); pusher->CreateFixture(&sd); sd.SetAsBox(2.f,1.5f,b2Vec2(-5.f,0.f),0.f); pusher->SetMassFromShapes(); pusher->CreateFixture(&sd); jd.Initialize(rightmotor,pusher,rightmotor->GetWorldCenter()+b2Vec2(-8.0f,0.f), pusher->GetWorldCenter()+b2Vec2(5.0f,0.f) ); m_world->CreateJoint(&jd); } /// Static bodies above motors { b2PolygonDef sd; b2CircleDef cd; sd.SetAsBox(9.0f, 0.5f); sd.friction = 0.05f; sd.restitution = 0.01f; b2BodyDef bd; bd.position.Set(-15.5f, 12.f); bd.angle = 0.0; b2Body* g = m_world->CreateBody(&bd); g->CreateFixture(&sd); sd.SetAsBox(8.f, 0.5f, b2Vec2(23.f,0.f),0.f); g->CreateFixture(&sd); /// compressor statics sd.SetAsBox(7.0f, 0.5f, b2Vec2(-2.f,9.f),0.f); g->CreateFixture(&sd); sd.SetAsBox(9.0f, 0.5f, b2Vec2(22.f,9.f),0.f); g->CreateFixture(&sd); sd.SetAsBox(19.0f, 0.5f, b2Vec2(-9.f,15.f),-0.05f); g->CreateFixture(&sd); sd.SetAsBox(4.7f, 0.5f, b2Vec2(15.f,11.5f),-0.5f); g->CreateFixture(&sd); /// below compressor sd.SetAsBox(26.0f, 0.3f, b2Vec2(17.f,-4.4f),-0.02f); g->CreateFixture(&sd); cd.radius = 1.0f; cd.friction = 1.0; cd.localPosition = b2Vec2(29.f,-6.f); g->CreateFixture(&cd); cd.radius = 0.7f; cd.localPosition = b2Vec2(-2.f,-4.5f); g->CreateFixture(&cd); } /// Elevator { b2BodyDef bd; b2CircleDef cd; b2PolygonDef sd; bd.position.Set(40.0f,4.0f); m_elev = m_world->CreateBody(&bd); sd.SetAsBox(0.5f, 2.5f,b2Vec2(3.0f,-3.0f), 0.f); sd.density = 1.f; sd.friction = 0.01f; m_elev->CreateFixture(&sd); sd.SetAsBox(7.0f, 0.5f, b2Vec2(-3.5f,-5.5f), 0.f); m_elev->CreateFixture(&sd); sd.SetAsBox(0.5f, 2.5f, b2Vec2(-11.f,-3.5f), 0.f); m_elev->CreateFixture(&sd); m_elev->SetMassFromShapes(); b2PrismaticJointDef jp; jp.Initialize(m_ground,m_elev, bd.position, b2Vec2(0.0f, 1.0f)); jp.lowerTranslation = 0.0f; jp.upperTranslation = 100.0f; jp.enableLimit = true; jp.enableMotor = true; jp.maxMotorForce = 10000.f; jp.motorSpeed = 0.f; m_joint_elev = (b2PrismaticJoint*)m_world->CreateJoint(&jp); /// Korb sd.SetAsBox(2.3f, 0.5f,b2Vec2(1.f,0.0f), 0.0f); sd.density = 0.5f; bd.position.Set(29.0f,6.5f); b2Body* body = m_world->CreateBody(&bd); body->CreateFixture(&sd); sd.SetAsBox(2.5f, 0.5f,b2Vec2(3.0f,-2.f), b2_pi/2.f); body->CreateFixture(&sd); sd.SetAsBox(4.6f, 0.5f,b2Vec2(7.8f,-4.0f), 0.f); body->CreateFixture(&sd); sd.SetAsBox(0.5f, 4.5f,b2Vec2(12.f,0.0f), 0.f); body->CreateFixture(&sd); sd.SetAsBox(0.5f, 0.5f,b2Vec2(13.f,4.0f), 0.f); body->CreateFixture(&sd); cd.radius = 0.7f; cd.density = 1.f; cd.friction = 0.01f; cd.localPosition = b2Vec2(0.f,0.f); body->CreateFixture(&cd); body->SetMassFromShapes(); b2RevoluteJointDef jr; jr.Initialize(m_elev,body, bd.position); jr.enableLimit = true; jr.lowerAngle = -0.2f; jr.upperAngle = b2_pi*1.1f; jr.collideConnected = true; m_world->CreateJoint(&jr); /// upper body exit sd.SetAsBox(14.0f, 0.5f,b2Vec2(-3.5f,-10.0f), 0.0f); bd.position.Set(17.5f,96.0f); body = m_world->CreateBody(&bd); body->CreateFixture(&sd); } /// "Elastic body" 64 bodies - something like a lin. elastic compound /// connected via dynamic forces (springs) { b2PolygonDef sd; sd.SetAsBox(0.55f, 0.55f); sd.density = 1.5f; sd.friction = 0.01f; sd.filter.groupIndex = -1; b2Vec2 startpoint(30.f,20.f); b2BodyDef bd; bd.isBullet = false; bd.allowSleep = false; for (int i = 0; i < 8; ++i) { for (int j = 0; j < 8; ++j) { bd.position.Set(j*1.02f, 2.51f + 1.02f * i); bd.position += startpoint; b2Body* body = m_world->CreateBody(&bd); bodies[8*i+j] = body; body->CreateFixture(&sd); body->SetMassFromShapes(); } } } } /// Apply dynamic forces (springs) and check elevator state void Step(Settings* settings) { Test::Step(settings); for (int i=0; i<8; ++i){ for (int j=0; j<8; ++j){ b2Vec2 zero(0.0f,0.0f); b2Vec2 down(0.0f, -0.5f); b2Vec2 up(0.0f, 0.5f); b2Vec2 right(0.5f, 0.0f); b2Vec2 left(-0.5f, 0.0f); int ind = i*8+j; int indr = ind+1; int indd = ind+8; float32 spring = 500.0f; float32 damp = 5.0f; if (j<7) { AddSpringForce(*(bodies[ind]),zero,*(bodies[indr]),zero,spring, damp, 1.0f); AddSpringForce(*(bodies[ind]),right,*(bodies[indr]),left,0.5f*spring, damp, 0.0f); } if (i<7) { AddSpringForce(*(bodies[ind]),zero,*(bodies[indd]),zero,spring, damp, 1.0f); AddSpringForce(*(bodies[ind]),up,*(bodies[indd]),down,0.5f*spring,damp,0.0f); } int inddr = indd + 1; int inddl = indd - 1; float32 drdist = sqrtf(2.0f); if (i < 7 && j < 7){ AddSpringForce(*(bodies[ind]),zero,*(bodies[inddr]),zero,spring, damp, drdist); } if (i < 7 && j > 0){ AddSpringForce(*(bodies[ind]),zero,*(bodies[inddl]),zero,spring, damp, drdist); } indr = ind+2; indd = ind+8*2; if (j<6) { AddSpringForce(*(bodies[ind]),zero,*(bodies[indr]),zero,spring, damp, 2.0f); } if (i<6) { AddSpringForce(*(bodies[ind]),zero,*(bodies[indd]),zero,spring,damp,2.0f); } inddr = indd + 2; inddl = indd - 2; drdist = sqrtf(2.0f)*2.0f; if (i < 6 && j < 6){ AddSpringForce(*(bodies[ind]),zero,*(bodies[inddr]),zero,spring, damp, drdist); } if (i < 6 && j > 1){ AddSpringForce(*(bodies[ind]),zero,*(bodies[inddl]),zero,spring, damp, drdist); } } } /// Check if bodies are near elevator /// Look if the body to lift is near the elevator b2Vec2 p1 = bodies[0]->GetWorldCenter(); b2Vec2 p2 = bodies[63]->GetWorldCenter(); /// m_elev: elevator prism. joint b2Vec2 e = m_elev->GetWorldCenter() + b2Vec2(0.f,7.f); // maybe not the best way to do it... // Bodies reached the elevator side if ( p1.x>e.x || p2.x>e.x ) { // go up if ( ( p1.y<e.y || p2.y<e.y ) && ( m_joint_elev->GetJointTranslation()<=m_joint_elev->GetLowerLimit()+1.f ) ) { m_joint_elev->SetMotorSpeed(20.f); //printf("lift goes up trans: %G\n",m_joint_elev->GetJointTranslation()); } } // go down if ( (m_joint_elev->GetJointTranslation()>=m_joint_elev->GetUpperLimit()-2.f) ) { m_joint_elev->SetMotorSpeed(-15.f); //printf("lift goes down: %G\n",m_joint_elev->GetJointTranslation()); } } /// Add a spring force void AddSpringForce(b2Body& bA, b2Vec2& localA, b2Body& bB, b2Vec2& localB, float32 k, float32 friction, float32 desiredDist) { b2Vec2 pA = bA.GetWorldPoint(localA); b2Vec2 pB = bB.GetWorldPoint(localB); b2Vec2 diff = pB - pA; //Find velocities of attach points b2Vec2 vA = bA.GetLinearVelocity() - b2Cross(bA.GetWorldVector(localA), bA.GetAngularVelocity()); b2Vec2 vB = bB.GetLinearVelocity() - b2Cross(bB.GetWorldVector(localB), bB.GetAngularVelocity()); b2Vec2 vdiff = vB-vA; float32 dx = diff.Normalize(); //normalizes diff and puts length into dx float32 vrel = vdiff.x*diff.x + vdiff.y*diff.y; float32 forceMag = -k*(dx-desiredDist) - friction*vrel; diff *= forceMag; // diff *= forceMag bB.ApplyForce(diff, bA.GetWorldPoint(localA)); diff *= -1.0f; bA.ApplyForce(diff, bB.GetWorldPoint(localB)); } /// Default constructor static Test* Create() { return new ElasticBody; } }; #endif