bool testBodyTreeModel1(unsigned numLevels, unsigned numBodies) { SharedPtr<Group> group = new Group("Group"); MobileRootJoint* mechanicRootJoint = new MobileRootJoint("RootJoint"); group->addChild(mechanicRootJoint); RigidBody* rigidBody = new RigidBody("RigidBody"); group->addChild(rigidBody); group->connect(mechanicRootJoint->getPort("link"), rigidBody->addLink("rootLink")); Mass* mass = new Mass("Mass", 1, InertiaMatrix(1, 0, 0, 1, 0, 1)); group->addChild(mass); group->connect(mass->getPort("link"), rigidBody->addLink("massLink")); addBodiesRecursive(numLevels, numBodies, rigidBody, group); SharedPtr<System> system = new System("Closed kinematic Loop 1"); system->setNode(group); if (!system->init()) { std::cerr << "Initialization of single body model failed!" << std::endl; return false; } return true; }
Node* buildDrop() { // A simple free falling mass. SharedPtr<Group> group = new Group("Group"); MobileRootJoint* mobileRootJoint = new MobileRootJoint("Root Joint"); mobileRootJoint->setInitialPosition(Vector3(0, 0, -10000)); mobileRootJoint->setInitialAngularVelocity(Vector3(1, 1, 1)); group->addChild(mobileRootJoint); RigidBody* rigidBody = new RigidBody("Rigid Body"); rigidBody->addLink("externalInteractLink"); group->addChild(rigidBody); Mass* mass = new Mass("Mass", 1, InertiaMatrix(1, 0, 0, 1, 0, 1)); group->addChild(mass); ExternalInteract* externalInteract = new ExternalInteract("ExternalInteract"); externalInteract->setPosition(mass->getPosition()); externalInteract->setEnableAllOutputs(true); group->addChild(externalInteract); group->connect(mobileRootJoint->getPort("link"), rigidBody->getPort("link0")); group->connect(rigidBody->getPort("link1"), mass->getPort("link")); group->connect(rigidBody->getPort("externalInteractLink"), externalInteract->getPort("link")); return group.release(); }
const dMass& Mass::createMass() { if(!created) { assembleMass(); for(std::list<SimObject*>::const_iterator iter = children.begin(), end = children.end(); iter != end; ++iter) { Mass* childMassDesc = dynamic_cast<Mass*>(*iter); ASSERT(childMassDesc); const dMass& childMass = childMassDesc->createMass(); if(childMassDesc->translation || childMassDesc->rotation) { dMass shiftedChildMass = childMass; if(childMassDesc->rotation) { dMatrix3 matrix; ODETools::convertMatrix(*childMassDesc->rotation, matrix); dMassRotate(&shiftedChildMass, matrix); } if(childMassDesc->translation) dMassTranslate(&shiftedChildMass, childMassDesc->translation->x, childMassDesc->translation->y, childMassDesc->translation->z); dMassAdd(&mass, &shiftedChildMass); } else dMassAdd(&mass, &childMass); } created = true; } return mass; }
void gravity_strip::init_strip() { length = 1.0f; thin_edges = true; curr_time = 0.0f; last_step_time = 0.0f; step_freq = 100.0f; offs = 0; num_lines = 1; vsx_vector v; /* color0[0] = 1; color0[1] = 1; color0[2] = 1; color0[3] = 1; color1[0] = 1; color1[1] = 1; color1[2] = 1; color1[3] = 1;*/ Mass m; m.init(v,v,7 / (rand() / (float)RAND_MAX * 2.5f + 0.35f)); masses.push_back(m); first = true; //masses[1].init(v,v, masses[0].mass+0.2f); }
void Mass::ApplyGravityFrom(const Mass body, float timestep) { float dx = this->position.X() - body.GetPosition().X(), dy = this->position.Y() - body.GetPosition().Y(), d = DistanceTo(body); if(d < 20) d = 20.0f; /* Accel due to gravity = GM / d^2 = d / t^2 We want change in velocity, which is d/t So multiply accel by t: GM / d^2 * t = GMt / d^2 unit: d/t But we want the components of that velocity So multiply by dx/d and dy/d: delta xv = GMt * dx / d^3 delta yv = GMt * dy / d^3 Calculate once GMt / d^3: */ float hertz = body.GetMass() * timestep * -0.0015 / (d*d*d); this->velocity = this->velocity + Vector2D(hertz*dx, hertz*dy); }
void update(){ updateCount++; if(updateCount%180==0) { screendump(imageCount,HEIGHT,WIDTH); imageCount++; } if(updateCount==2000){ //masses[0]->pinned = 0; //masses[width-1]->pinned = 0; //masses[width*width-1]->pinned = 0; //masses[width*(width-1)]->pinned = 0; masses[(int)((width*.5)*width+(width*.5))]->pinned = 0; } for(int i = 0; i<masses.size(); i++){ Mass *m = masses[i]; if(updateCount){ m->pinned = 0; } m->update(.03f); float damp = .8f; if(m->position.x()<-2+r) m->velocity.x() = (damp*abs(m->velocity.x())); if(m->position.x()>2-r) m->velocity.x() = -(damp*abs(m->velocity.x())); if(m->position.y()<-2+r) m->velocity.y() = (damp*abs(m->velocity.y())); if(m->position.y()>2-r) m->velocity.y() = -(damp*abs(m->velocity.y())); if(m->position.z()<-2+r) m->velocity.z() = (damp*abs(m->velocity.z())); if(m->position.z()>2-r) m->velocity.z() = -(damp*abs(m->velocity.z())); } for(int i = 0; i<masses.size(); i++){ Mass *m = masses[i]; m->commit(); } display(); }
/**检查碰撞*/ bool Garment::collision(NetGrid * netGrid){ float gap = netGrid->gap; float tmplength = netGrid->length; float tmpwidth = netGrid->width; float tmpheight = netGrid->height; //先求出当前点在netgrid中的位置 在判断该位置是否为1 int allClothLength = allCloth.size(); int clothLength =0; //判断是否碰撞 碰了重置速度 Mass * tmpMass; //FILE *dataFile2; //读入的文件 //dataFile2 = fopen("gai_out.txt", "w"); for(int i = 0;i < allClothLength; i ++){ clothLength = allCloth.at(i).getVectorMass()->size(); for(int j = 0;j < clothLength;j ++){ tmpMass = allCloth.at(i).getVectorMass()->at(j); float x =allCloth.at(i).getVectorMass()->at(j)->x + allCloth.at(i).getVectorMass()->at(j)->massSpeed.getX() * 0.005f; float y =allCloth.at(i).getVectorMass()->at(j)->y + allCloth.at(i).getVectorMass()->at(j)->massSpeed.getY() * 0.005f; float z =allCloth.at(i).getVectorMass()->at(j)->z + allCloth.at(i).getVectorMass()->at(j)->massSpeed.getZ() * 0.005f; int tmpx = (x + tmplength/2 )/gap; int tmpy = (156- y)/gap; int tmpz = (tmpwidth/2 - z)/gap; if(isIn[tmpx][tmpz][tmpy]){ //重置速度 tmpMass->resetVelocity(); } } } /*fprintf(dataFile2,"\n\n\n"); fclose(dataFile2);*/ return false; }
void addBodiesRecursive(unsigned numLevels, unsigned numBodies, RigidBody* rigidBody, Group* group) { if (!numLevels) return; for (unsigned i = 0; i < numBodies; ++i) { RevoluteJoint* revoluteJoint = new RevoluteJoint("RevoluteJoint"); group->addChild(revoluteJoint); group->connect(revoluteJoint->getPort("link0"), rigidBody->addLink("childLink")); RigidBody* rigidBody2 = new RigidBody("RigidBody"); group->addChild(rigidBody2); group->connect(revoluteJoint->getPort("link1"), rigidBody2->addLink("parentLink")); Mass* mass = new Mass("Mass", 1, InertiaMatrix(1, 0, 0, 1, 0, 1)); group->addChild(mass); group->connect(mass->getPort("link"), rigidBody2->addLink("massLink")); addBodiesRecursive(numLevels - 1, numBodies, rigidBody2, group); } }
Node* buildSimpleMechanicExample() { SharedPtr<Group> group = new Group("G"); MobileRootJoint* mobileRootJoint = new MobileRootJoint("Root Joint"); group->addChild(mobileRootJoint); RigidBody *rigidBody = new RigidBody("Rigid Body"); rigidBody->addLink("link2"); rigidBody->addLink("externalInteractLink"); rigidBody->addLink("internalInteractLink"); rigidBody->addLink("internalInteractLink2"); group->addChild(rigidBody); InertiaMatrix inertia(1, 0, 0, 1, 0, 1); Mass* mass = new Mass("Mass", 1, inertia); group->addChild(mass); RevoluteJoint* revoluteJoint = new RevoluteJoint("Revolute Joint"); revoluteJoint->setEnableExternalForce(true); group->addChild(revoluteJoint); RigidBody *rigidBody2 = new RigidBody("Rigid Body 2"); rigidBody2->addLink("externalInteractLink"); rigidBody2->addLink("internalInteractLink"); rigidBody2->addLink("internalInteractLink2"); group->addChild(rigidBody2); Mass* mass2 = new Mass("Mass 2", 1, inertia); group->addChild(mass2); ExternalInteract* externalInteract = new ExternalInteract("ExternalInteract"); externalInteract->setPosition(mass->getPosition()); externalInteract->setEnableAllOutputs(true); group->addChild(externalInteract); ExternalInteract* externalInteract2 = new ExternalInteract("ExternalInteract 2"); externalInteract2->setPosition(mass2->getPosition()); externalInteract2->setEnableAllOutputs(true); group->addChild(externalInteract2); group->connect(mobileRootJoint->getPort("link"), rigidBody->getPort("link0")); group->connect(rigidBody->getPort("link1"), mass->getPort("link")); group->connect(rigidBody->getPort("link2"), revoluteJoint->getPort("link0")); group->connect(revoluteJoint->getPort("link1"), rigidBody2->getPort("link0")); group->connect(rigidBody2->getPort("link1"), mass2->getPort("link")); group->connect(rigidBody->getPort("externalInteractLink"), externalInteract->getPort("link")); group->connect(rigidBody2->getPort("externalInteractLink"), externalInteract2->getPort("link")); ConstModel* jointForce = new ConstModel("Joint Force", 1); group->addChild(jointForce); group->connect(jointForce->getPort("output"), revoluteJoint->getPort("force")); InternalInteract* internalInteract = new InternalInteract("InternalInteract"); internalInteract->setPosition0(Vector3(0, 0, 1)); internalInteract->setPosition1(Vector3(0, 0, 0.8)); internalInteract->setEnableAllOutputs(true); internalInteract->setEnableForce(true); group->addChild(internalInteract); group->connect(internalInteract->getPort("link0"), rigidBody->getPort("internalInteractLink")); group->connect(internalInteract->getPort("link1"), rigidBody2->getPort("internalInteractLink")); InternalInteract* internalInteract2 = new InternalInteract("InternalInteract2"); internalInteract2->setPosition0(Vector3(0, 0, 0.8)); internalInteract2->setPosition1(Vector3(0, 0, 1)); internalInteract2->setEnableAllOutputs(true); group->addChild(internalInteract2); group->connect(internalInteract2->getPort("link1"), rigidBody->getPort("internalInteractLink2")); group->connect(internalInteract2->getPort("link0"), rigidBody2->getPort("internalInteractLink2")); LinearSpringDamper* damper = new LinearSpringDamper("LinearSpringDamper"); damper->setSpringConstant(0.5); damper->setDamperConstant(1); group->addChild(damper); group->connect(damper->getPort("velocity"), internalInteract->getPort("velocity")); group->connect(damper->getPort("position"), internalInteract->getPort("distance")); group->connect(damper->getPort("force"), internalInteract->getPort("force")); return group.release(); }
double getForce(){ return stiffness * (left.getPosition() - right.getPosition()); };
int main(int argc, char *argv[]) { real_type omega = 2; SharedPtr<System> system = new System("Tire Testrig"); system->addSampleTime(real_type(1)/real_type(100)); system->setTimestepper(new DoPri5); // set the moving ground system->getEnvironment()->setGround(new MovingGround(Vector3(10, 0, 0))); system->getEnvironment()->setGravity(new ZGravity()); // First build up the mechanical system FixedRootJoint* fixedRootJoint = new FixedRootJoint("Fixed Root Joint"); system->addModel(fixedRootJoint); fixedRootJoint->setRefPosition(Vector3(0, 0, 1)); PrismaticJoint* prismaticJoint = new PrismaticJoint("Normal Force joint"); prismaticJoint->setJointAxis(Vector3::unit(2)); system->addModel(prismaticJoint); Summer* normalForceSum = new Summer("Normal Force Sum"); normalForceSum->setNumSummands(2); normalForceSum->setInputSign(0, Summer::Minus); system->addModel(normalForceSum); Connection::connect(prismaticJoint->getInputPort(0), normalForceSum->getOutputPort(0)); ConstModel* normalForce = new ConstModel("Normal force"); normalForce->setScalarValue(2000); system->addModel(normalForce); Connection::connect(normalForceSum->getInputPort(0), normalForce->getOutputPort(0)); LinearSpringDamper* strutDamper = new LinearSpringDamper("Strut Damper"); strutDamper->setSpringConstant(0); strutDamper->setDamperConstant(-30); system->addModel(strutDamper); Connection::connect(normalForceSum->getInputPort(1), strutDamper->getOutputPort(0)); Connection::connect(strutDamper->getInputPort(0), prismaticJoint->getOutputPort(0)); Connection::connect(strutDamper->getInputPort(1), prismaticJoint->getOutputPort(1)); RigidBody* rootMount = new RigidBody("Root Mount"); system->addModel(rootMount); rootMount->setInboardJoint(fixedRootJoint); rootMount->addInteract(prismaticJoint); RevoluteActuator* camberActuator = new RevoluteActuator("Camber Actuator"); system->addModel(camberActuator); ConstModel* camberAngle = new ConstModel("Camber Angle"); camberAngle->setScalarValue(0); system->addModel(camberAngle); Connection::connect(camberActuator->getInputPort(0), camberAngle->getOutputPort(0)); RigidBody* normalForceStrut = new RigidBody("Normal Force Strut"); system->addModel(normalForceStrut); normalForceStrut->setInboardJoint(prismaticJoint); normalForceStrut->addInteract(camberActuator); RevoluteActuator* sideActuator = new RevoluteActuator("Sideslip Actuator"); system->addModel(sideActuator); ConstModel* sideslipAngle = new ConstModel("Sideslip Angle"); sideslipAngle->setScalarValue(0); system->addModel(sideslipAngle); Connection::connect(sideActuator->getInputPort(0), sideslipAngle->getOutputPort(0)); RigidBody* camberStrut = new RigidBody("Camber Strut"); system->addModel(camberStrut); camberStrut->setInboardJoint(camberActuator); camberStrut->addInteract(sideActuator); RevoluteJoint* hubJoint = new RevoluteJoint("Hub Joint"); hubJoint->setJointAxis(Vector3(0, 1, 0)); system->addModel(hubJoint); RigidBody* hubStrut = new RigidBody("Hub Strut"); system->addModel(hubStrut); hubStrut->setInboardJoint(sideActuator); hubStrut->addInteract(hubJoint); RigidBody* rimAndTire = new RigidBody("Rim And Tire"); system->addModel(rimAndTire); rimAndTire->setInboardJoint(hubJoint); Mass* tireAndRimMass = new Mass("Rim And Tire Mass"); tireAndRimMass->setInertia(1, InertiaMatrix(1, 0, 0, 1, 0, 1)); system->addModel(tireAndRimMass); rimAndTire->addInteract(tireAndRimMass); WheelContact* wheelContact = new WheelContact("Wheel Contact"); wheelContact->setWheelRadius(0.3); wheelContact->setSpringConstant(1000); wheelContact->setSpringDamping(sqrt(wheelContact->getSpringConstant())/10); system->addModel(wheelContact); rimAndTire->addInteract(wheelContact); if (!system->init()) { std::cout << "Could not initialize the system" << std::endl; return EXIT_FAILURE; } HDF5Writer hwriter("system.h5"); system->accept(hwriter); while (system->getTime() < 10) { system->simulate(system->getTime() + 0.01); system->accept(hwriter); } return EXIT_SUCCESS; }