/** * * @param localPosition */ void TriangleGeom::setLocalOrientation(const Quaternion &localOrientation) { for(int i = 0; i < mPoints.size(); i++) { mPoints.replace(i, (mPoints.at(i) - mLocalPosition)); } Vector3D newPoint; Quaternion back(mLocalOrientation); back.setW(-back.getW()); Quaternion inv = back.getInverse(); Quaternion old(0, 0, 0, 0); for(int i = 0; i < mPoints.size(); i++) { old.set(0, mPoints.at(i).getX(), mPoints.at(i).getY(), mPoints.at(i).getZ()); Quaternion newPointQ = back * old *inv; newPoint.set(newPointQ.getX(), newPointQ.getY(), newPointQ.getZ()); mPoints.replace(i, newPoint); } Quaternion inverse = localOrientation.getInverse(); Quaternion oldPoint(0, 0, 0, 0); for(int i = 0; i < mPoints.size(); i++) { oldPoint.set(0,mPoints.at(i).getX(), mPoints.at(i).getY(), mPoints.at(i).getZ()); Quaternion newPointQ = localOrientation * oldPoint *inverse; newPoint.set(newPointQ.getX(), newPointQ.getY(), newPointQ.getZ()); mPoints.replace(i, newPoint); } for(int i = 0; i < mPoints.size(); i++) { mPoints.replace(i, (mPoints.at(i) + mLocalPosition)); } SimGeom::setLocalOrientation(localOrientation); }
Player(int team, double xx, double yy){ //Obj::Obj(xx,yy,7); this->team_id = team; if(xx>0)p.set(-2*( field_x-xx)+field_x, yy*1.4, 0.0); else p.set(-2*(-field_x-xx)-field_x, yy*1.4, 0.0); r.set(xx,yy,10); v.set(0.0,0.0,0.0); a.set(0.0,0.0,0.0); if(r.x>=0) goal.set( -field_x, 0.0, 0.0); else if(r.x<0) goal.set( field_x, 0.0, 0.0); if(r.x>=0)theta = 3.14; else if(r.x<0)theta = 0.01; d.set( cos(theta), sin(theta), 0.0); accel = ds_theta =0; switch (this->team_id){ case 0:setColor(0.7,0.0,0.0);break; case 1:setColor(0.0,0.0,0.7);break; case 2:setColor(1.0,1.0,1.0);break; } radius=4; m=100; mode=WAIT; camera=0; }
void keyboard(unsigned char key, int x, int y) { // 90% of keyboard input will be movement, and c++ complains about // declarations within case statements, so calculate zoom and strafe // vector here even if we're not going to use it Vector3D zoom = look - eye; zoom.scaleToOne(); Vector3D strafe = zoom.crossProd(Vector3D(0,1,0)); Vector3D up = zoom.crossProd(strafe); switch( key) { case 'w': case 'W': // forward look += zoom * SCALE_FACTOR; eye += zoom * SCALE_FACTOR; break; case 'a': case 'A': // left strafe look -= strafe * SCALE_FACTOR; eye -= strafe * SCALE_FACTOR; break; case 's': case 'S': // back look -= zoom * SCALE_FACTOR; eye -= zoom * SCALE_FACTOR; break; case 'd': case 'D': // right strafe look += strafe * SCALE_FACTOR; eye += strafe * SCALE_FACTOR; break; case 'r' : case 'R' : glMatrixMode(GL_MODELVIEW); glPushMatrix(); glLoadIdentity(); glPopMatrix(); eye.set(0.0, 1.0, 3.0); look.set(0.0, 0.0, 0.0); phi = -15.0; theta = 272.0; glutPostRedisplay(); break; case 'q': case 'Q': exit(0); break; } // Also since 90% of keyboard input is movement, might as well do this // all the time and save myself some code lines if (eye.y < 1.0) { eye.y = 1.0; look.y = eye.y + sin(deg2rad(phi)); } glutPostRedisplay(); }
void ExtendoHand::getHeading(Vector3D &m) { if (nineAxis) { #ifdef ENABLE_MAGNETOMETER int16_t mx, my, mz; magnet.getHeading(&mx, &my, &mz); m.set(mx, my, mz); #else m.set(0, 0, 0); #endif // ENABLE_MAGNETOMETER } else { m.set(0, 0, 0); } }
void ExtendoHand::getRotation(Vector3D &g) { if (nineAxis) { #ifdef ENABLE_GYRO int16_t gx, gy, gz; gyro.getRotation(&gx, &gy, &gz); g.set(gx, gy, gz); #else g.set(0, 0, 0); #endif // ENABLE_GYRO } else { g.set(0, 0, 0); } }
void setUp() { // sets up the unit vectors e1.set(1,0,0); e2.set(0,1,0); e3.set(0,0,1); mIdentity = " 1 0 0 " " 0 1 0 " " 0 0 1"; mMat1 = " 1 4.2 2 " " 5.2 3 4 " " 1 7 0.4"; mMat1sym = " 1 4.2 2 " " 4.2 3 4 " " 2 4 0.4"; mMat2 = " 2 4 5 " " 2 1 3 " " 0 7 8 "; mMat2trans = " 2 2 0 " " 4 1 7 " " 5 3 8 "; mMat1plus2 = " 3 8.2 7 " " 7.2 4 7 " " 1 14 8.4"; mMat1plus2trans = " 3 6.2 2 " " 9.2 4 11 " " 6 10 8.4"; mMat1times2 = " 10.4 22.2 33.6 " " 16.4 51.8 67.0 " " 16.0 13.8 29.2 "; mMat1times2inv = " 2.519141 0.088216 -0.977863 " " 1.009487 0.304594 -0.593375 " " 5.838881 -0.699068 -2.203728 "; tolEps = 1e-12; }
void setUp() { tolEps=1.e-12; // sets up the unit vectors e1.set(1,0,0); e2.set(0,1,0); e3.set(0,0,1); MsqPrintError err(cout); // creates empty Patch create_one_hex_patch(oneHexPatch, err); CPPUNIT_ASSERT(!err); create_one_tet_patch(oneTetPatch, err); CPPUNIT_ASSERT(!err); create_one_tri_patch(oneTriPatch, err); CPPUNIT_ASSERT(!err); create_one_quad_patch(oneQuadPatch, err); CPPUNIT_ASSERT(!err); }
void move(){ theta += ds_theta; d.set( cos(theta), sin(theta), 0.0); a = accel * d; Obj::move(); double dd = (v.x*d.y-d.x*v.y) / v.getNorm(); v.x *= (19+dd)/20; v.y *= (19+dd)/20; }
void PointDomain::closest_point( Mesh::VertexHandle , const Vector3D& , Vector3D& closest, Vector3D& normal, MsqError& err ) const { closest = geom(); normal.set(0,0,0); MSQ_SETERR(err)( "Cannot get normal for PointDomain", MsqError::INTERNAL_ERROR ); }
void setUp() { tolEps=1.e-12; // set up the unit vectors e1.set(1,0,0); e2.set(0,1,0); e3.set(0,0,1); MsqPrintError err(cout); double hcoords[] = { 1.0, 1.0, 1.0, 2.0, 1.0, 1.0, 2.0, 2.0, 1.0, 1.0, 2.0, 1.0, 1.0, 1.0, 2.0, 2.0, 1.0, 2.0, 2.0, 2.0, 2.0, 1.0, 2.0, 2.0 }; size_t hconn[] = { 0, 1, 2, 3, 4, 5, 6, 7 }; one_hex_patch.fill( 8, hcoords, 1, HEXAHEDRON, hconn, 0, err ); double tcoords[] = { 1.0, 1.0, 1.0, 2.0, 1.0, 1.0, 1.5, 1+sqrt(3.0)/2.0, 1.0, 1.5, 1+sqrt(3.0)/6.0, 1+sqrt(2.0)/sqrt(3.0) }; size_t tconn[] = { 0, 1, 2, 3 }; one_tet_patch.fill( 4, tcoords, 1, TETRAHEDRON, tconn, 0, err ); double qcoords[] = { 1.0, 1.0, 1.0, 2.0, 1.0, 1.0, 2.0, 2.0, 1.0, 1.0, 2.0, 1.0 }; size_t qconn[] = { 0, 1, 2, 3 }; one_qua_patch.fill( 4, qcoords, 1, QUADRILATERAL, qconn, 0, err ); double rcoords[] = { 1.0, 1.0, 1.0, 2.0, 1.0, 1.0, 1.5, 1+sqrt(3.0)/2.0, 1.0 }; size_t rconn[] = { 0, 1, 2 }; one_tri_patch.fill( 3, rcoords, 1, TRIANGLE, rconn, 0, err ); }
void ExtendoHand::getAcceleration(Vector3D &a) { if (nineAxis) { int16_t ax, ay, az; accel.getAcceleration(&ax, &ay, &az); // approximate g values, per calibration with a specific sensor a.set( ax / 230.0 - 0.05, ay / 230.0, az / 230.0); } else { #ifdef THREE_AXIS a.set( motionSensor.accelX(), motionSensor.accelY(), motionSensor.accelZ()); #endif // THREE_AXIS } }
/** * Creates a new ODE_HingeJoint. * * @param body1 the first body to connect the joint to. * @param body2 the second body to connect the joint to. * @return the new ODE_HingeJoint. */ dJointID ODE_HingeJoint::createJoint(dBodyID body1, dBodyID body2) { if(mJointAxisPoint1->get().equals(mJointAxisPoint2->get())) { Core::log("Invalid axes for ODE_HingeJoint."); return 0; } //if one of the bodyIDs is null, the joint is connected to a static object. dJointID newJoint = dJointCreateHinge(mWorldID, mGeneralJointGroup); dJointAttach(newJoint, body1, body2); Vector3D anchor = mJointAxisPoint1->get() ; Vector3D axis = mJointAxisPoint2->get() ; axis.set(mJointAxisPoint2->getX() - mJointAxisPoint1->getX(), mJointAxisPoint2->getY() - mJointAxisPoint1->getY(), mJointAxisPoint2->getZ() - mJointAxisPoint1->getZ()); dJointSetHingeAnchor(newJoint, anchor.getX(), anchor.getY(), anchor.getZ()); dJointSetHingeAxis(newJoint, axis.getX(), axis.getY(), axis.getZ()); return newJoint; }
/** * Class constructor with initialize parameters. * @param entityMesh is scene entity geometry data. * @param center is bounding sphere center - entity position. */ BoundingSphere::BoundingSphere(const Mesh& entityMesh, const Vector3D& center) { position = center; float currentDistance = 0.0f; float maxDistance = 0.0f; Vertex<>* verts = entityMesh.getVertices(); Vector3D temp; for(int i = 0; i < entityMesh.getVerticesAmount(); ++i) { temp.set(verts[i].x,verts[i].y,verts[i].z); currentDistance = (temp - center).length(); if(currentDistance > maxDistance) maxDistance = currentDistance; } sphereRadius = sqrt(maxDistance); }
/** * Private method which is used to recursivly build OctTree nodes starting from root. * @param parent is pointer to new node parent. */ void OctTree::buildNode(OctNode* parent) { if(condition == MINENTITY && parent->entities.size() > 1) { Vector3D position; position.set(parent->position.x()+parent->size.x()*0.5f,parent->position.y()+parent->size.y()*0.5f,parent->position.z()+parent->size.z()*0.5f); createNodeChildren(parent,position,0); buildNode(parent->children[0]); position.set(parent->position.x()-parent->size.x()*0.5f,parent->position.y()-parent->size.y()*0.5f,parent->position.z()-parent->size.z()*0.5f); createNodeChildren(parent,position,1); buildNode(parent->children[1]); position.set(parent->position.x()+parent->size.x()*0.5f,parent->position.y()+parent->size.y()*0.5f,parent->position.z()-parent->size.z()*0.5f); createNodeChildren(parent,position,2); buildNode(parent->children[2]); position.set(parent->position.x()+parent->size.x()*0.5f,parent->position.y()-parent->size.y()*0.5f,parent->position.z()+parent->size.z()*0.5f); createNodeChildren(parent,position,3); buildNode(parent->children[3]); position.set(parent->position.x()-parent->size.x()*0.5f,parent->position.y()+parent->size.y()*0.5f,parent->position.z()+parent->size.z()*0.5f); createNodeChildren(parent,position,4); buildNode(parent->children[4]); position.set(parent->position.x()-parent->size.x()*0.5f,parent->position.y()-parent->size.y()*0.5f,parent->position.z()+parent->size.z()*0.5f); createNodeChildren(parent,position,5); buildNode(parent->children[5]); position.set(parent->position.x()+parent->size.x()*0.5f,parent->position.y()-parent->size.y()*0.5f,parent->position.z()-parent->size.z()*0.5f); createNodeChildren(parent,position,6); buildNode(parent->children[6]); position.set(parent->position.x()-parent->size.x()*0.5f,parent->position.y()+parent->size.y()*0.5f,parent->position.z()-parent->size.z()*0.5f); createNodeChildren(parent,position,7); buildNode(parent->children[7]); } }
void PointDomain::vertex_normal_at( Mesh::VertexHandle , Vector3D &coordinate) const { coordinate.set(0,0,0); }
dJointID ODE_Dynamixel::createJoint(dBodyID body1, dBodyID body2) { if(mJointAxisPoint1->get().equals(mJointAxisPoint2->get(), -1)) { Core::log("ODE_Dynamixel: Invalid axes for ODE_Dynamixel.", true); return 0; } Vector3D anchor = mJointAxisPoint1->get(); Vector3D axis = mJointAxisPoint2->get() - mJointAxisPoint1->get(); dJointID joint = dJointCreateAMotor(mWorldID, mGeneralJointGroup); dJointAttach(joint, body1, body2); dJointSetAMotorMode(joint, dAMotorEuler); dJointSetAMotorParam(joint, dParamVel, 0.0); dJointSetAMotorParam(joint, dParamFMax, mDesiredMotorTorqueValue->get()); dJointSetAMotorParam(joint, dParamCFM, mCFMValue->get()); dJointSetAMotorParam(joint, dParamStopERP, mStopERPValue->get()); dJointSetAMotorParam(joint, dParamStopCFM, mStopCFMValue->get()); dJointSetAMotorParam(joint, dParamBounce, mBounceValue->get()); dJointSetAMotorParam(joint, dParamFudgeFactor, mFudgeFactorValue->get()); axis.normalize(); Vector3D perpedicular; if(axis.getY() != 0.0 || axis.getX() != 0.0) { perpedicular.set(-axis.getY(), axis.getX(), 0); } else { perpedicular.set(0, -axis.getZ(), axis.getY()); } perpedicular.normalize(); // If one of the bodies is static, the motor axis need to be defined in a different way. For different constellations of static and dynamic bodies, the turn direction of the motor changed, so this had to be added. if(body1 == 0) { dJointSetAMotorAxis(joint, 0, 0, -axis.getX(), -axis.getY(), -axis.getZ()); } else { dJointSetAMotorAxis(joint, 0, 1, axis.getX(), axis.getY(), axis.getZ()); } if(body2 == 0) { dJointSetAMotorAxis(joint, 2, 0, -perpedicular.getX(), -perpedicular.getY(), -perpedicular.getZ()); } else { dJointSetAMotorAxis(joint, 2, 2, perpedicular.getX(), perpedicular.getY(), perpedicular.getZ()); } mHingeJoint = dJointCreateHinge(mWorldID, mGeneralJointGroup); dJointAttach(mHingeJoint, body1, body2); dJointSetHingeAnchor(mHingeJoint, anchor.getX(), anchor.getY(), anchor.getZ()); dJointSetHingeAxis(mHingeJoint, axis.getX(), axis.getY(), axis.getZ()); if(body1 == 0) { double tmp = mMinAngle; mMinAngle = -1.0 * mMaxAngle; mMaxAngle = -1.0 * tmp; } dJointSetHingeParam(mHingeJoint, dParamLoStop, mMinAngle); dJointSetHingeParam(mHingeJoint, dParamHiStop, mMaxAngle); dJointSetHingeParam(mHingeJoint, dParamVel, 0.0); return joint; }
void XYRectangle::element_normal_at( Mesh::ElementHandle handle, Vector3D &norm ) const { norm.set(0,0,0); norm[normalDir] = 1.0; }
/** * Method is used to create axis vectors from transposed matrix. * @param x is new xAxis vector. * @param y is new yAxis vector. * @param z is new zAxis vector. */ void Matrix4::toAxesTransposed(Vector3D& x, Vector3D& y, Vector3D& z) const { x.set(matrix[0][0], matrix[1][0], matrix[2][0]); y.set(matrix[0][1], matrix[1][1], matrix[2][1]); z.set(matrix[0][2], matrix[1][2], matrix[2][2]); }
Tank::Tank() : mTankSize(1.5, 1.0, 1.5), mHeadCenter(0.0, 0.75, 0.0), mHeadSize(0.75, 0.5, 0.75), mTurretCenter(0.0, 0.0, 0.75), mTurretSize(0.25, 0.25, 0.75) { //setRadius(0.5); mRadius = 0.75; mWhatAmI = Entity::TANK; mAlive = true; mPosition[0] = mTerrainWidth / 2; mPosition[1] = 0.5; mPosition[2] = mTerrainHeight / 2; mTankSpeed = 0.15f; mTankTurnRate = 4.0f; mHeadRotationRate = 4.0f; mTurretConstantRotate = false; mAIRotateCalled = false; mCurrentMoveRate = 0; mCurrentRotationRate = 0; mHeadRotationDirection = 0; mHeadTargetDirection = 0; mHeadRotation = 0; mFrontLeftControl.set(0 + 0.75f, -0.5f, 0 + 0.75f); mFrontRightControl.set(0 - 0.75f, -0.5f, 0 + 0.75f); mBackLeftControl.set(0 + 0.75f, -0.5f, 0 - 0.75f); mBackRightControl.set(0 - 0.75f, -0.5f, 0 - 0.75f); float a = 0.5; GLfloat baseRect[24] = {-a, a, a, //0 0 a, a, a, //1 3 a, a, -a, //2 6 -a, a, -a, //3 9 -a, -a, a, //4 12 a, -a, a, //5 15 a, -a, -a, //6 18 -a, -a, -a}; //7 21 for (int i = 0; i < 24; ++i) { mBaseRect[i] = baseRect[i]; } Vector3D<GLfloat> nextNormal; GLfloat normals[24]; //mBaseRectNormals = new GLfloat[24]; for (int i = 0; i < 24; i += 3) { nextNormal.set(baseRect[i], baseRect[i + 1], baseRect[i + 2]); nextNormal.normalize(); mBaseRectNormals[i] = nextNormal[0]; mBaseRectNormals[i + 1] = nextNormal[1]; mBaseRectNormals[i + 2] = nextNormal[2]; normals[i] = nextNormal[0]; normals[i + 1] = nextNormal[1]; normals[i + 2] = nextNormal[2]; } //mBaseRectNormals = normals; mNumIndices = 24; GLuint indices[24] = {4, 5, 1, 0, 5, 6, 2, 1, 6, 7, 3, 2, 7, 4, 0, 3, 0, 1, 2, 3, 4, 7, 6, 5}; //mBaseRectIndices = new GLuint[mNumIndices]; for (int i = 0; i < mNumIndices; ++i) { mBaseRectIndices[i] = indices[i]; } mBody = ModelStack::load("bradley_body.c3m"); mHead = ModelStack::load("bradley_head.c3m"); mTurret = ModelStack::load("bradley_turret.c3m"); }
void Interstitial::evaluate(const ISO& iso, const Symmetry& symmetry, int numPointsPerAtom, double tol, double scale) { // Clear space clear(); // Output Output::newline(); Output::print("Searching for interstitial sites using "); Output::print(numPointsPerAtom); Output::print(" starting point"); if (numPointsPerAtom != 1) Output::print("s"); Output::print(" per atom and a scale of "); Output::print(scale); Output::increase(); // Constants used in generating points on sphere around each atom double phiScale = Constants::pi * (3 - sqrt(5)); double yScale = 2.0 / numPointsPerAtom; // Loop over unique atoms in the structure int i, j, k; int count = 0; double y; double r; double phi; double curDistance; double nearDistance; double startDistance; Vector3D curPoint; Linked<Vector3D > points; for (i = 0; i < symmetry.orbits().length(); ++i) { // Get the distance to nearest atom in the structure nearDistance = -1; for (j = 0; j < iso.atoms().length(); ++j) { for (k = 0; k < iso.atoms()[j].length(); ++k) { curDistance = iso.basis().distance(symmetry.orbits()[i].atoms()[0]->fractional(), FRACTIONAL, \ iso.atoms()[j][k].fractional(), FRACTIONAL); if (curDistance > 0) { if ((nearDistance == -1) || (curDistance < nearDistance)) nearDistance = curDistance; } } } // Set the starting distance away from atom startDistance = nearDistance / 2; // Loop over starting points for (j = 0; j < numPointsPerAtom; ++j) { // Check if running current point on current processor if ((++count + Multi::rank()) % Multi::worldSize() == 0) { // Get current starting point y = j * yScale - 1 + (yScale / 2); r = sqrt(1 - y*y); phi = j * phiScale; curPoint.set(symmetry.orbits()[i].atoms()[0]->cartesian()[0] + startDistance*r*cos(phi), \ symmetry.orbits()[i].atoms()[0]->cartesian()[1] + startDistance*y, \ symmetry.orbits()[i].atoms()[0]->cartesian()[2] + startDistance*r*sin(phi)); // Minimize the current point if (!minimizePoint(curPoint, iso, scale)) continue; // Save current point in fractional coordinates points += iso.basis().getFractional(curPoint); ISO::moveIntoCell(*points.last()); } } } // Reduce list of points to unique ones int m; bool found; int numLoops; Vector3D rotPoint; Vector3D equivPoint; Vector3D origin(0.0); Linked<double> distances; Linked<double>::iterator itDist; Linked<Vector3D> uniquePoints; Linked<Vector3D>::iterator it; Linked<Vector3D>::iterator itUnique; for (i = 0; i < Multi::worldSize(); ++i) { // Send number of points in list on current processor numLoops = points.length(); Multi::broadcast(numLoops, i); // Loop over points if (i == Multi::rank()) it = points.begin(); for (j = 0; j < numLoops; ++j) { // Send out current point if (i == Multi::rank()) { curPoint = *it; ++it; } Multi::broadcast(curPoint, i); // Get current distance to origin curDistance = iso.basis().distance(curPoint, FRACTIONAL, origin, FRACTIONAL); // Loop over points that were already saved found = false; itDist = distances.begin(); itUnique = uniquePoints.begin(); for (; itDist != distances.end(); ++itDist, ++itUnique) { // Current points are not the same if (Num<double>::abs(curDistance - *itDist) <= tol) { if (iso.basis().distance(curPoint, FRACTIONAL, *itUnique, FRACTIONAL) <= tol) { found = true; break; } } // Loop over symmetry operations for (k = 0; k < symmetry.operations().length(); ++k) { // Loop over translations rotPoint = symmetry.operations()[k].rotation() * curPoint; for (m = 0; m < symmetry.operations()[k].translations().length(); ++m) { // Check if points are the same equivPoint = rotPoint; equivPoint += symmetry.operations()[k].translations()[m]; if (iso.basis().distance(equivPoint, FRACTIONAL, *itUnique, FRACTIONAL) <= tol) { found = true; break; } } if (found) break; } if (found) break; } // Found a new point if (!found) { distances += curDistance; uniquePoints += curPoint; } } } // Save unique points _sites.length(uniquePoints.length()); for (i = 0, it = uniquePoints.begin(); it != uniquePoints.end(); ++i, ++it) _sites[i] = *it; // Output Output::newline(); Output::print("Found "); Output::print(_sites.length()); Output::print(" possible interstitial site"); if (_sites.length() != 1) Output::print("s"); Output::increase(); for (i = 0; i < _sites.length(); ++i) { Output::newline(); Output::print("Site "); Output::print(i+1); Output::print(":"); for (j = 0; j < 3; ++j) { Output::print(" "); Output::print(_sites[i][j], 8); } } Output::decrease(); // Output Output::decrease(); }
dJointID ODE_PID_PassiveActuatorModel::createJoint(dBodyID body1, dBodyID body2) { Vector3DValue *jointAxisPoint1 = dynamic_cast<Vector3DValue*>(mOwner->getParameter("AxisPoint1")); Vector3DValue *jointAxisPoint2 = dynamic_cast<Vector3DValue*>(mOwner->getParameter("AxisPoint2")); DoubleValue *minAngleValue = dynamic_cast<DoubleValue*>(mOwner->getParameter("MinAngle")); DoubleValue *maxAngleValue = dynamic_cast<DoubleValue*>(mOwner->getParameter("MaxAngle")); if(jointAxisPoint1 == 0 || jointAxisPoint2 == 0 || minAngleValue == 0 || maxAngleValue == 0) { Core::log("ODE_PID_PassiveActuatorModel: Could not find all required parameter values."); return 0; } if(jointAxisPoint1->get().equals(jointAxisPoint2->get(), -1)) { Core::log("ODE_PID_PassiveActuatorModel: Invalid axis points " + jointAxisPoint1->getValueAsString() + " and " + jointAxisPoint2->getValueAsString() + "."); return 0; } if(jointAxisPoint1->get().equals(jointAxisPoint2->get(), -1)) { Core::log("Invalid axes for ODE_PID_PassiveActuatorModel."); return 0; } Vector3D anchor = jointAxisPoint1->get(); Vector3D axis = jointAxisPoint2->get() - jointAxisPoint1->get(); dJointID joint = dJointCreateAMotor(mWorldID, mGeneralJointGroup); dJointAttach(joint, body1, body2); dJointSetAMotorMode(joint, dAMotorEuler); dJointSetAMotorParam(joint, dParamVel, 0.0); dJointSetAMotorParam(joint, dParamFMax, 1.0); dJointSetAMotorParam(joint, dParamCFM, mCFMValue->get()); dJointSetAMotorParam(joint, dParamStopERP, mStopERPValue->get()); dJointSetAMotorParam(joint, dParamStopCFM, mStopCFMValue->get()); dJointSetAMotorParam(joint, dParamBounce, mBounceValue->get()); dJointSetAMotorParam(joint, dParamFudgeFactor, mFudgeFactorValue->get()); axis.normalize(); Vector3D perpedicular; if(axis.getY() != 0.0 || axis.getX() != 0.0) { perpedicular.set(-axis.getY(), axis.getX(), 0); } else { perpedicular.set(0, -axis.getZ(), axis.getY()); } perpedicular.normalize(); // If one of the bodies is static, the motor axis need to be defined in a different way. For different constellations of static and dynamic bodies, the turn direction of the motor changed, so this had to be added. if(body1 == 0) { dJointSetAMotorAxis(joint, 0, 0, -axis.getX(), -axis.getY(), -axis.getZ()); } else { dJointSetAMotorAxis(joint, 0, 1, axis.getX(), axis.getY(), axis.getZ()); } if(body2 == 0) { dJointSetAMotorAxis(joint, 2, 0, -perpedicular.getX(), -perpedicular.getY(), -perpedicular.getZ()); } else { dJointSetAMotorAxis(joint, 2, 2, perpedicular.getX(), perpedicular.getY(), perpedicular.getZ()); } mHingeJoint = dJointCreateHinge(mWorldID, mGeneralJointGroup); dJointAttach(mHingeJoint, body1, body2); dJointSetHingeAnchor(mHingeJoint, anchor.getX(), anchor.getY(), anchor.getZ()); dJointSetHingeAxis(mHingeJoint, axis.getX(), axis.getY(), axis.getZ()); double minAngle = (minAngleValue->get() * Math::PI) / 180.0; double maxAngle = (maxAngleValue->get() * Math::PI) / 180.0; if(body1 == 0) { double tmp = minAngle; minAngle = -1.0 * maxAngle; maxAngle = -1.0 * tmp; } dJointSetHingeParam(mHingeJoint, dParamLoStop, minAngle); dJointSetHingeParam(mHingeJoint, dParamHiStop, maxAngle); dJointSetHingeParam(mHingeJoint, dParamVel, 0.0); return joint; }
void XYRectangle::vertex_normal_at( Mesh::VertexHandle handle, Vector3D &norm ) const { norm.set(0,0,0); norm[normalDir] = 1.0; }
void PointDomain::element_normal_at( Mesh::ElementHandle , Vector3D &coordinate) const { coordinate.set(0,0,0); }
void Robot01BlockCode::processLocalEvent(EventPtr pev) { stringstream info; MessagePtr message; switch (pev->eventType) { case EVENT_TRANSLATION_END: robotBlock->setColor(LIGHTBLUE); info.str(""); info << robotBlock->blockId << " rec.: EVENT_TRANSLATION_END"; scheduler->trace(info.str(),hostBlock->blockId); // prepare for next motion nbreOfWaitedAnswers=0; block2Answer=NULL; if (blockToUnlock!=0) { sendUnlockMessage(blockToUnlock); } else { // dernier élément du train if (trainNext==NULL) { info.str(""); info << "rerun " ;//<< trainNextId << "," << trainPreviousId; scheduler->trace(info.str(),hostBlock->blockId); robotBlock->setColor(DARKORANGE); trainPrevious=NULL; PointRel3D pt; calcPossibleMotions(pt); sendReLinkTrainMessage(); } else { info.str(""); info << "ready " ;//<< trainNextId << "," << trainPreviousId; scheduler->trace(info.str(),hostBlock->blockId); robotBlock->setPrevNext(trainPrevious,trainNext); robotBlock->setColor(BLUE); } } break; case EVENT_NI_RECEIVE: message = (std::static_pointer_cast<NetworkInterfaceReceiveEvent>(pev))->message; P2PNetworkInterface * recvInterface = message->destinationInterface; switch(message->id) { case MAP_MSG_ID : { MapMessage_ptr recvMessage = std::static_pointer_cast<MapMessage>(message); unsigned int sourceId = recvMessage->sourceInterface->hostBlock->blockId; // cout << "INTERFACE" << robotBlock->getDirection(message->destinationInterface) << endl; info.str(""); info << "rec. MapMessage : MAP_MSG from " << sourceId; scheduler->trace(info.str(),hostBlock->blockId); if (targetGrid) { sendAckMap(recvInterface); } else { // first message memcpy(gridSize,recvMessage->gridSize,3*sizeof(short)); targetGrid = initGrid(gridSize,recvMessage->targetGrid); block2Answer=recvInterface; nbreOfWaitedAnswers=0; sendMapToNeighbors(block2Answer); if (nbreOfWaitedAnswers==0) { sendAckMap(block2Answer); block2Answer=NULL; info.str(""); info << " the end"; scheduler->trace(info.str(),hostBlock->blockId,GOLD); currentTrainGain=0; PointRel3D pt; calcPossibleMotions(pt); } } } break; case ACKMAP_MSG_ID : { AckMapMessage_ptr recvMessage = std::static_pointer_cast<AckMapMessage>(message); unsigned int sourceId = recvMessage->sourceInterface->hostBlock->blockId; info.str(""); info << "rec. AckMapMessage(" << nbreOfWaitedAnswers << ") from " << sourceId; scheduler->trace(info.str(),hostBlock->blockId,LIGHTBLUE); nbreOfWaitedAnswers--; if (nbreOfWaitedAnswers==0) { if (block2Answer!=NULL) { sendAckMap(block2Answer); block2Answer=NULL; info.str(""); info << "waits for train message"; } else { // you are master block because NULL father info.str(""); info << " next step"; } scheduler->trace(info.str(),hostBlock->blockId,GOLD); currentTrainGain=0; PointRel3D pos; pos.x = robotBlock->position[0]; pos.y = robotBlock->position[1]; pos.z = robotBlock->position[2]; goodPlace = targetGrid[(pos.z*gridSize[1]+pos.y)*gridSize[0]+pos.x]==fullCell; PointRel3D pt; calcPossibleMotions(pt); } } break; case TRAIN_MSG_ID : { TrainMessage_ptr recvMessage = std::static_pointer_cast<TrainMessage>(message); unsigned int sourceId = recvMessage->sourceInterface->hostBlock->blockId; info.str(""); info << "rec. TrainMessage (" << recvMessage->newPos << "," << recvMessage->gain <<") from " << sourceId; currentTrainGain+=recvMessage->gain; currentTrainGoal= recvMessage->newPos; info << "\ncurrentGain = " << currentTrainGain; scheduler->trace(info.str(),hostBlock->blockId); trainPrevious = recvMessage->sourceInterface; robotBlock->setPrevNext(trainPrevious,trainNext); if (block2Answer==NULL) { calcPossibleMotions(recvMessage->newPos); sendLinkTrainMessages(recvMessage->destinationInterface); } else { stringstream info; info.str(""); info << "block2answer!=NULL"; scheduler->trace(info.str(),hostBlock->blockId,RED); } } break; case ACKTRAIN_MSG_ID : { AckTrainMessage_ptr recvMessage = std::static_pointer_cast<AckTrainMessage>(message); unsigned int sourceId = recvMessage->sourceInterface->hostBlock->blockId; info.str(""); info << "rec. AckTrainMessage("<< recvMessage->answer << ") from " << sourceId; scheduler->trace(info.str(),hostBlock->blockId); // 2 situations à gérer : // - le voisin ne peut pas se déplacer // - le bloc est en tête if (recvMessage->answer && trainPrevious!=NULL) { AckTrainMessage *message = new AckTrainMessage(true); scheduler->schedule(new NetworkInterfaceEnqueueOutgoingEvent(scheduler->now() + COM_DELAY, message, trainPrevious->connectedInterface)); info.str(""); info << "send AckTrainMessage(true) to " << trainPrevious->connectedInterface->connectedInterface->hostBlock->blockId; scheduler->trace(info.str(),hostBlock->blockId,GREEN); } else { // gestion de l'échec if (!recvMessage->answer) { trainPrevious=NULL; if (possibleMotions && !possibleMotions->empty()) { // il y a eu échec sur la premiere règle // on essaye la suivante : Validation *firstCondition = possibleMotions->back(); possibleMotions->pop_back(); delete firstCondition; info.str(""); info << possibleMotions->size(); std::reverse_iterator<vector<Validation*>::iterator> rev_until (possibleMotions->begin()); std::reverse_iterator<vector<Validation*>::iterator> rev_from (possibleMotions->end()); while (rev_from!=rev_until) { info << "/" << (*rev_from)->capa->isHead << ":" << (*rev_from)->capa->isEnd << " " << (*rev_from)->capa->name << " gain=" << (*rev_from)->gain; rev_from++; } scheduler->trace(info.str(),hostBlock->blockId,GOLD); /***********************************/ /* IL FAUT TESTER SI C'EST UNE FIN */ if (!possibleMotions->empty()) { sendLinkTrainMessages(trainNext); } /*else { sendLinkTrainMessages(trainNext,NULL); trainNext=NULL; robotBlock->setPrevNext(trainPrevious,trainNext); }*/ } else { // sendLinkTrainMessages(block2Answer); } } else { info.str(""); info << "Head of train :" << robotBlock->blockId << ", next=" << trainNext->hostBlock->blockId; //info << "\n" << robotBlock->blockId << " mv(" << motionVector.x << "," << motionVector.y << "," << motionVector.z << ")" // << " nmv(" << nextMotionVector.x << "," << nextMotionVector.y << "," << nextMotionVector.z << ")"; scheduler->trace(info.str(),hostBlock->blockId,GREEN); block2Answer=NULL; sendAnswerDelayOrMotionDelayMessage(scheduler->now()-10*COM_DELAY); } } } break; case MOTIONDELAY_MSG_ID : { MotionDelayMessage_ptr recvMessage = std::static_pointer_cast<MotionDelayMessage>(message); unsigned int sourceId = recvMessage->sourceInterface->hostBlock->blockId; info.str(""); info << robotBlock->blockId << " rec. MotionDelayMsg(" << recvMessage->unlockMode << ") from " << sourceId; scheduler->trace(info.str(),hostBlock->blockId,GREEN); info.str(""); info << robotBlock->blockId << " mv(" << motionVector.x << "," << motionVector.y << "," << motionVector.z << ")" << " nmv(" << nextMotionVector.x << "," << nextMotionVector.y << "," << nextMotionVector.z << ")"; scheduler->trace(info.str(),hostBlock->blockId,GREEN); if (recvMessage->unlockMode) { trainPrevious=NULL; robotBlock->setPrevNext(trainPrevious,trainNext); } sendAnswerDelayOrMotionDelayMessage(recvMessage->globalTime); } break; case ANSWERDELAY_MSG_ID : { AnswerDelayMessage_ptr recvMessage = std::static_pointer_cast<AnswerDelayMessage>(message); unsigned int sourceId = recvMessage->sourceInterface->hostBlock->blockId; info.str(""); info << robotBlock->blockId << " rec. AnswerDelayMsg(" << recvMessage->globalRDVTime << ") from " << sourceId; scheduler->trace(info.str(),hostBlock->blockId); Vector3D finalPosition; finalPosition.set(robotBlock->position.pt[0]+motionVector.x, robotBlock->position.pt[1]+motionVector.y,robotBlock->position.pt[2]+motionVector.z); blockToUnlock=0; scheduler->schedule(new TranslationStartEvent(recvMessage->globalRDVTime,robotBlock,finalPosition)); stringstream info; info.str(""); info << robotBlock->blockId << " TranslationStartEvent(" << recvMessage->globalRDVTime << ")"; scheduler->trace(info.str(),hostBlock->blockId,LIGHTGREY); if (trainPrevious) { AnswerDelayMessage *adm_message = new AnswerDelayMessage(recvMessage->globalRDVTime,false); scheduler->schedule(new NetworkInterfaceEnqueueOutgoingEvent(scheduler->now() + COM_DELAY, adm_message, trainPrevious->connectedInterface)); stringstream info; info.str(""); info << robotBlock->blockId << " send AnswerDelayMsg("<< adm_message->globalRDVTime << ") to " << trainPrevious->hostBlock->blockId; scheduler->trace(info.str(),hostBlock->blockId,GREEN); } } break; case UNLOCK_MSG_ID : { UnlockMessage_ptr recvMessage = std::static_pointer_cast<UnlockMessage>(message); unsigned int sourceId = recvMessage->sourceInterface->hostBlock->blockId; info.str(""); info << robotBlock->blockId << " rec. UnlockMessage(" << recvMessage->target << ") from " << sourceId; scheduler->trace(info.str(),hostBlock->blockId); // search if target is directly connected to the block int i=0; P2PNetworkInterface *p2p = robotBlock->getInterface(SCLattice::Direction(i)); bool found=(p2p->connectedInterface && p2p->connectedInterface->hostBlock->blockId==recvMessage->target); //cout <<(p2p->connectedInterface?p2p->connectedInterface->hostBlock->blockId:-1) << endl; while (i<6 && !found) { i++; if (i<6) { p2p = robotBlock->getInterface(SCLattice::Direction(i)); found=(p2p->connectedInterface && p2p->connectedInterface->hostBlock->blockId==recvMessage->target); //cout <<(p2p->connectedInterface?p2p->connectedInterface->hostBlock->blockId:-1) << endl; } } if (found) { Time time = scheduler->now(); MotionDelayMessage *message = new MotionDelayMessage(time,true); scheduler->schedule(new NetworkInterfaceEnqueueOutgoingEvent(time + COM_DELAY, message, p2p)); stringstream info; info.str(""); info << robotBlock->blockId << " send MotionDelayMsg(unlock) to " << p2p->connectedInterface->hostBlock->blockId; scheduler->trace(info.str(),hostBlock->blockId,GREEN); } } break; /************************** case RELINKTRAIN_MSG_ID : { ReLinkTrainMessage_ptr recvMessage = std::static_pointer_cast<ReLinkTrainMessage>(message); unsigned int sourceId = recvMessage->sourceInterface->hostBlock->blockId; info.str(""); info << " rec. ReLinkTrainMessage() from " << sourceId; scheduler->trace(info.str(),hostBlock->blockId,GREEN); trainNext=message->destinationInterface; trainPrevious=NULL; currentTrainGain=0; PointRel3D pt; calcPossibleMotions(pt); sendReLinkTrainMessage(); } break;*/ default : cerr << "Block " << hostBlock->blockId << " received an unrecognized message from " << message->sourceInterface->hostBlock->blockId << endl; break; } break; } robotBlock->setColor((trainPrevious?(trainNext?MAGENTA:PINK):(trainNext?RED:goodPlace?GREEN:YELLOW))); }