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;
}
