コード例 #1
0
ファイル: TriangleGeom.cpp プロジェクト: nerd-toolkit/nerd
/**
 * 
 * @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);
}
コード例 #2
0
ファイル: Player.hpp プロジェクト: routerman/OpenGL3DSample
	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;
	}
コード例 #3
0
ファイル: program4.cpp プロジェクト: mgius/cpe471
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();
}
コード例 #4
0
ファイル: ExtendoHand.cpp プロジェクト: XixiLuo/extendo
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);
    }
}
コード例 #5
0
ファイル: ExtendoHand.cpp プロジェクト: XixiLuo/extendo
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);
    }
}
コード例 #6
0
ファイル: Matrix3DTest.cpp プロジェクト: haripandey/trilinos
  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;
  }
コード例 #7
0
  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);
  }
コード例 #8
0
ファイル: Player.hpp プロジェクト: routerman/OpenGL3DSample
	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;
	}
コード例 #9
0
ファイル: MeshDomain1D.cpp プロジェクト: haripandey/trilinos
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 );
}
コード例 #10
0
ファイル: MsqVertexTest.cpp プロジェクト: haripandey/trilinos
  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 );
  }
コード例 #11
0
ファイル: ExtendoHand.cpp プロジェクト: XixiLuo/extendo
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
    }
}
コード例 #12
0
ファイル: ODE_HingeJoint.cpp プロジェクト: nerd-toolkit/nerd
/**
 * 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; 
}
コード例 #13
0
/**
 * 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);
}
コード例 #14
0
ファイル: OctTree.cpp プロジェクト: veldrinlab/ayumiEngine
		/**
		 * 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]);
			}
		}
コード例 #15
0
ファイル: MeshDomain1D.cpp プロジェクト: haripandey/trilinos
void PointDomain::vertex_normal_at( Mesh::VertexHandle ,
                                    Vector3D &coordinate) const
  { coordinate.set(0,0,0); }
コード例 #16
0
ファイル: ODE_Dynamixel.cpp プロジェクト: nerd-toolkit/nerd
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;
}
コード例 #17
0
void XYRectangle::element_normal_at( Mesh::ElementHandle handle, Vector3D &norm ) const
{
  norm.set(0,0,0);
  norm[normalDir] = 1.0;
}
コード例 #18
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]);
}
コード例 #19
0
ファイル: Tank.cpp プロジェクト: CYBORUS/paroxysm
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");
}
コード例 #20
0
ファイル: interstitial.cpp プロジェクト: bbucior/mint
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();
}
コード例 #21
0
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;
}
コード例 #22
0
void XYRectangle::vertex_normal_at( Mesh::VertexHandle handle, Vector3D &norm ) const
{
  norm.set(0,0,0);
  norm[normalDir] = 1.0;
}
コード例 #23
0
ファイル: MeshDomain1D.cpp プロジェクト: haripandey/trilinos
void PointDomain::element_normal_at( Mesh::ElementHandle ,
                                     Vector3D &coordinate) const
  { coordinate.set(0,0,0); }
コード例 #24
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)));
}