Пример #1
0
	Matrix4 FPSview(Vector3 cameraPosition, float pitch, float yaw)
	{
		// If the pitch and yaw angles are in degrees,
		// they need to be converted to radians. Here
		// I assume the values are already converted to radians.
		float cosPitch = cos(pitch);
		float sinPitch = sin(pitch);
		float cosYaw = cos(yaw);
		float sinYaw = sin(yaw);
 
		Vector3 xaxis( cosYaw, 0, -sinYaw);
		Vector3 yaxis( sinYaw * sinPitch, cosPitch, cosYaw * sinPitch);
		Vector3 zaxis( sinYaw * cosPitch, -sinPitch, cosPitch * cosYaw);
 
		// Create a 4x4 view matrix from the right, up, forward and eye position vectors
		Matrix4 viewMatrix 
		( 
		    Vector4(				   xaxis.x,					 yaxis.x,				   zaxis.x,     0 ),
		    Vector4(				   xaxis.y,					 yaxis.y,				   zaxis.y,     0 ),
		    Vector4(				   xaxis.z,					 yaxis.z,				   zaxis.z,     0 ),
		    Vector4(-xaxis.dot(cameraPosition), -yaxis.dot(cameraPosition), -zaxis.dot(cameraPosition), 1 )
		);
		 
		return viewMatrix;
	}
Пример #2
0
S2Matrix4x4 Camera::GetViewMatrix() const {
	const Transform &trans = *GetTransform();

	//Use spherical coordinate for forward vector.
	const S2Vector3 & rotate = trans.GetRotate();
	float theta = rotate[0], phi = rotate[1];
	phi = phi > 1.57f ? 1.57f : phi;
	phi = phi < -1.57f ? -1.57f : phi;

	S2Vector3 zaxis(sin(theta)*cos(phi), sin(phi), cos(theta)*cos(phi));
	S2Vector3 xaxis = S2Vector3(0.0f, 1.0f, 0.0f).Cross(zaxis);
	xaxis.Normalize();
	S2Vector3 yaxis = zaxis.Cross(xaxis);

	S2Vector3 position = trans.GetTranslate();
	float p_dot_x = position.Dot(xaxis);
	float p_dot_y = position.Dot(yaxis);
	float p_dot_z = position.Dot(zaxis);
	
	return S2Matrix4x4(
		xaxis[0],  				xaxis[1], 				xaxis[2],			-p_dot_x,
		yaxis[0],				yaxis[1], 				yaxis[2],			-p_dot_y,
		zaxis[0], 				zaxis[1], 				zaxis[2],			-p_dot_z,
		0.0f,						0.0f,						0.0f,					1.0f
	);
}
Пример #3
0
void
avtExtrudeFilter::SetAtts(const AttributeGroup *a)
{
    atts = *(const ExtrudeAttributes*)a;

    avtVector axis(atts.GetAxis()), zaxis(0.,0.,1.);
    
    if ((axis.x == 0. && axis.y == 0. && axis.z == 0.)
#if 0
        || zaxis.dot(axis) == 0.
#endif
       )
    {
        EXCEPTION1(BadVectorException, "Extrusion Axis");
    }

    if(atts.GetLength() <= 0.)
    {
        EXCEPTION1(ImproperUseException, "Length must be greater than 0.");
    }

    if(atts.GetSteps() < 1)
    {
        EXCEPTION1(ImproperUseException, "Steps must be at least 1.");
    }
}
Пример #4
0
void LLCamera::calculateWorldFrustumPlanes() 
{
	F32 d;
	LLVector3 center = mOrigin - mXAxis*mNearPlane;
	mWorldPlanePos = center;
	for (int p=0; p<4; p++)
	{
		LLVector3 pnorm = LLVector3(mLocalPlanes[p]);
		LLVector3 norm = rotateToAbsolute(pnorm);
		norm.normVec();
		d = -(center * norm);
		mWorldPlanes[p] = LLPlane(norm, d);
	}
	// horizontal planes, perpindicular to (0,0,1);
	LLVector3 zaxis(0, 0, 1.0f);
	F32 yaw = getYaw();
	{
		LLVector3 tnorm = LLVector3(mLocalPlanes[PLANE_LEFT]);
		tnorm.rotVec(yaw, zaxis);
		d = -(mOrigin * tnorm);
		mHorizPlanes[HORIZ_PLANE_LEFT] = LLPlane(tnorm, d);
	}
	{
		LLVector3 tnorm = LLVector3(mLocalPlanes[PLANE_RIGHT]);
		tnorm.rotVec(yaw, zaxis);
		d = -(mOrigin * tnorm);
		mHorizPlanes[HORIZ_PLANE_RIGHT] = LLPlane(tnorm, d);
	}
}
Пример #5
0
void CBillBoard::onRender(const D3DXMATRIX& mView, const D3DXVECTOR3& vPos) {
	CDirect3D::getInstance()->AlphaTestEnable(TRUE);
	CDirect3D::getInstance()->AlphaFunction(D3DCMP_GREATER);
	CDirect3D::getInstance()->AlphaReference(0x80);
	CDirect3D::getInstance()->SetD3DFVF(SVertexT::FVF);
	CDirect3D::getInstance()->SetStreamSource(0, m_pVB, 0, sizeof(SVertexT));
	CDirect3D::getInstance()->SetTexture(0, m_pTexture);
	D3DXVECTOR3 vUp(mView._12, mView._22, mView._32);
	/* simpler, yet supposed-to-be less efficient *
	D3DXMatrixLookAtRH(&m_matWorld, &m_vPos, &vPos, &vUp); // Backward Direction
	m_matWorld._41 = m_matWorld._42 = m_matWorld._43 = 0.0f;
	D3DXMatrixTranspose(&m_matWorld, &m_matWorld);

	/* optimized by doing manually */
	D3DXVECTOR3 zaxis(m_vPos - vPos); D3DXVec3Normalize(&zaxis, &zaxis);
	D3DXVECTOR3 xaxis; D3DXVec3Normalize(D3DXVec3Cross(&xaxis, &vUp, &zaxis), &xaxis);
	D3DXVECTOR3 yaxis; D3DXVec3Cross(&yaxis, &zaxis, &xaxis);
	m_matWorld._11 = xaxis.x; m_matWorld._12 = xaxis.y; m_matWorld._13 = xaxis.z;
	m_matWorld._21 = yaxis.x; m_matWorld._22 = yaxis.y; m_matWorld._23 = yaxis.z;
	m_matWorld._31 = zaxis.x; m_matWorld._32 = zaxis.y; m_matWorld._33 = zaxis.z;
	/**/
	m_matWorld._41 = m_vPos.x; m_matWorld._42 = m_vPos.y; m_matWorld._43 = m_vPos.z;
	CDirect3D::getInstance()->SetTransform(D3DTS_WORLD, &m_matWorld);
	CDirect3D::getInstance()->DrawPrimitive(D3DPT_TRIANGLESTRIP, 0, 2);
	CDirect3D::getInstance()->SetTexture(0, NULL);
	CDirect3D::getInstance()->AlphaTestEnable(FALSE);
}
Пример #6
0
void
StarRegionScalarInteractor::updateScale()
{
   osg::Matrix scaleMat, rotMat, offsetMat;
   scaleMat.makeScale (interScale/cover->getScale(), interScale/cover->getScale(), interScale/cover->getScale());

   osg::Vec3 n;
   n = localMat * interNormal;

   rotMat.makeRotate (defAxis, n);
   scaleMat.makeScale (interScale/cover->getScale(), interScale/cover->getScale(), interScale/cover->getScale());
   offsetMat.mult (scaleMat, rotMat);

   osg::Vec3 p;
   p=interPos;
   float dr=interScale/cover->getScale();
   //p[2]+= -regionRadius - dr -4*dindex*dr;

   osg::Vec3 zaxis (0,0,1);
   zaxis = localMat * zaxis;

   p+=zaxis* (-regionRadius - dr -4*dindex*dr);

   /*   offsetMat.setRow(3, p); */
   for (int i = 0; i < 3; i++)
   {
      offsetMat (3, i) = p[i];
   }

   worldDCS->setMatrix (offsetMat);

}
Пример #7
0
void rgl_bbox(int* successptr,
              int* idata,
              double* ddata,
              double* xat, char** xtext,
              double* yat, char** ytext,
              double* zat, char** ztext)
{
  int success = RGL_FAIL;

  Device* device = deviceManager->getAnyDevice();

  if (device) {

    int   xticks     =        idata[0];
    int   yticks     =        idata[1];
    int   zticks     =        idata[2];
    int   xlen       =        idata[3];
    int   ylen       =        idata[4];
    int   zlen       =        idata[5];
    int   marklen_rel =       idata[6];

    float xunit      = (float) ddata[0];
    float yunit      = (float) ddata[1];
    float zunit      = (float) ddata[2];
    float marklen    = (float) ddata[3];

    AxisInfo xaxis(xticks, xat, xtext, xlen, xunit);
    AxisInfo yaxis(yticks, yat, ytext, ylen, yunit);
    AxisInfo zaxis(zticks, zat, ztext, zlen, zunit);

    success = as_success( device->add( new BBoxDeco(currentMaterial, xaxis, yaxis, zaxis, marklen, (bool) marklen_rel ) ) );
  }

  *successptr = success;
}
ofVec3f GrainViewTransform(ofVec3f InPutPoint, ofVec3f Eye, ofVec3f Target, ofVec3f UpVec){

// Transform the cordinates of a point InPutPoint. For a camera placed at Eye and looking at target with Up vector UpVec
    
    ofMatrix4x4 ViewMat;
    ofVec3f zaxis(Target - Eye);
    zaxis.normalize();
    ofVec3f xaxis = UpVec.getCrossed(zaxis);
    xaxis.normalize();
    ofVec3f yaxis = zaxis.getCrossed(xaxis);
    // translate vect
    
    InPutPoint -= Eye;
    
    // project on each axis
    
    ofVec3f Outpos;
    
    Outpos.x = -xaxis.dot(InPutPoint);
    Outpos.y = -yaxis.dot(InPutPoint);
    Outpos.z = zaxis.dot(InPutPoint);
    
    return Outpos;
    
}
Пример #9
0
Camera & Camera::TranslateForward(float distance) {
	Transform &trans = *GetTransform();
	//Use spherical coordinate for forward vector.
	const S2Vector3 & rotate = trans.GetRotate();
	float theta = rotate[0], phi = rotate[1];
	phi = phi > 1.57f ? 1.57f : phi;
	phi = phi < -1.57f ? -1.57f : phi;

	S2Vector3 zaxis(sin(theta)*cos(phi), sin(phi), cos(theta)*cos(phi));

	trans.Translate(zaxis * distance);

	return *this;
}
Пример #10
0
AtomStickInteractor::AtomStickInteractor(string symbol, const char *interactorName, Atom *myAtom, osg::Matrix initialOrientation, osg::Vec3 initialPos, osg::Vec3 stickDir, float size, osg::Vec4 color)
    : coVR3DRotCenterInteractor(initialOrientation, initialPos, size, coInteraction::ButtonA, "Hand", interactorName, coInteraction::Medium)
{
    if (opencover::cover->debugLevel(3))
        fprintf(stderr, "AtomStickInteractor::AtomStickInteractor\n");

    initialOrientation_ = initialOrientation;
    initialPos_ = initialPos;
    dir_ = stickDir;
    dir_.normalize();
    symbol_ = symbol;
    color_ = color;
    myAtom_ = myAtom;

    osg::Vec3 zaxis(0, 0, 1);
    osg::Matrix mr;
    mr.makeRotate(zaxis, dir_);
    mr.postMultTranslate(dir_ * 0.5 * _interSize);
    // cylinder of lengths size, beginning at center of sphere,
    //sphere is 0,5*size so also 0.5*size of the cylinder comes out of the spehere
    osg::Cylinder *myCylinder = new osg::Cylinder(osg::Vec3(0, 0, 0), 0.2 * _interSize, _interSize);
    osg::TessellationHints *hint = new osg::TessellationHints();
    hint->setDetailRatio(0.5);
    osg::ShapeDrawable *cylinderDrawable = new osg::ShapeDrawable(myCylinder, hint);
    cylinderDrawable->setColor(osg::Vec4(color[0], color[1], color[2], color[3]));
    osg::Geode *cylinderGeode = new osg::Geode();
    osg::StateSet *normalState = opencover::VRSceneGraph::instance()->loadDefaultGeostate(osg::Material::AMBIENT_AND_DIFFUSE);
    cylinderGeode->setStateSet(normalState);

    transform_ = new osg::MatrixTransform();
    transform_->setMatrix(mr);
    cylinderGeode->addDrawable(cylinderDrawable);
    transform_->addChild(cylinderGeode);

    //coVRIntersectionInteractor::geometryNode
    geometryNode = transform_;

    // remove old geometry
    scaleTransform->removeChild(0, scaleTransform->getNumChildren());

    // add geometry to node and remove old scaling
    scaleTransform->addChild(transform_);
    osg::Matrix s;
    scaleTransform->setMatrix(s);

    connectedStick_ = NULL; //not connected

    oldInvMat_.invert(getMatrix());
}
Пример #11
0
inline void FXGLVertices::renderLines(FXGLViewer *viewer, bool isHit, bool complex){
#ifdef HAVE_GL_H
  FXGLColor col(color);
  GLUquadricObj* quad=0;
  if(complex){
    quad=gluNewQuadric();
    gluQuadricDrawStyle(quad,(GLenum)GLU_FILL);
  }
  FXuint inc=(!(options & (SHADING_SMOOTH|SHADING_FLAT)) && viewer->doesTurbo()) ? 4 : 1;
  for(FXuint n=0; n<vertexNumber-complex; n+=inc){
    if(!isHit){
      if(colorGenerator) for(FXuint c=0; c<inc; c++) colorGenerator(col, this, viewer, n, colorGeneratorData);
      if(complex && (options & (SHADING_SMOOTH|SHADING_FLAT))){
        glMaterialfv(GL_FRONT_AND_BACK,GL_DIFFUSE,&col.r);
        }
       else
        glColor4fv(&col.r);
      }
    if(complex){
      FXVec3f vector=vertices[n+1]-vertices[n];
      FXfloat len=vector.length();
      if(!(options & VERTICES_POINTS) || len>=pointSize*0.8f){
        glPushMatrix();
        glTranslatef(vertices[n].x,vertices[n].y,vertices[n].z);
        FXVec3f zaxis(0.0f,0.0f,vector.z<0 ? 1.0f : -1.0f);
        // Cylinder points in Z direction, so we need to rotate so it follows vector
        FXVec3f vectornormal(vecnormalize(vector));
        FXVec3f axis(vectornormal^zaxis);
        FXfloat angle=((FXfloat) RTOD)*asinf(axis.length());
        //FXfloat dotproduct=vectornormal*axis;
        if(vector.z<0) angle+=180;
        glRotatef(angle,axis.x,axis.y,axis.z);
        gluCylinder(quad,lineSize/2,lineSize/2,len,8,8);
        glPopMatrix();
        }
      }
    else{
      glVertex3fv(&vertices[n].x);
      }
    }
  if(complex){
    gluDeleteQuadric(quad);
  }
#endif
  }
Пример #12
0
	Matrix4x4 Matrix4x4::LookTo(const Vector3 &eye_position, const Vector3 &to_direction, const Vector3 &up_direction)
	{
		Matrix4x4 m = Matrix4x4::Identity();
    
		Vector3 zaxis(-to_direction);
		zaxis.Normalize();

		Vector3 xaxis = zaxis * up_direction;
		xaxis.Normalize();

		Vector3 yaxis = xaxis * zaxis;

		m.m00 = xaxis.x;	m.m01 = xaxis.y;	m.m02 = xaxis.z;		m.m03 = -xaxis.Dot(eye_position);
		m.m10 = yaxis.x;	m.m11 = yaxis.y;	m.m12 = yaxis.z;		m.m13 = -yaxis.Dot(eye_position);
		m.m20 = zaxis.x;	m.m21 = zaxis.y;	m.m22 = zaxis.z;		m.m23 = -zaxis.Dot(eye_position);
		m.m30 = 0;			m.m31 = 0;			m.m32 = 0;				m.m33 = 1.0f;

		return m;
	}
Пример #13
0
int MyCRCLServer::HandleMoveToType(MoveToType *cmd)
{
  double x, y, z;
  double xi, xj, xk;
  double zi, zj, zk;
  double r, p, w;

  x = cmd->EndPosition->Point->X->val;
  y = cmd->EndPosition->Point->Y->val;
  z = cmd->EndPosition->Point->Z->val;

  xi = cmd->EndPosition->XAxis->I->val;
  xj = cmd->EndPosition->XAxis->J->val;
  xk = cmd->EndPosition->XAxis->K->val;
  tf::Vector3 xaxis(xi, xj, xk);
  zi = cmd->EndPosition->ZAxis->I->val;
  zj = cmd->EndPosition->ZAxis->J->val;
  zk = cmd->EndPosition->ZAxis->K->val;
  tf::Vector3 zaxis(zi, zj, zk);
  tf::Vector3 yaxis = zaxis.cross(xaxis);
  tf::Matrix3x3 m(xi, yaxis.getX(), zi,
		  xj, yaxis.getY(), zj,
		  xk, yaxis.getZ(), zk);
  m.getRPY(r, p, w);

  status.setXYZ(x, y, z);
  status.setVec(xi, xj, xk, zi, zj, zk);
  status.setJointPosition(1, x);
  status.setJointPosition(2, y);
  status.setJointPosition(3, z);
  status.setJointPosition(4, r);
  status.setJointPosition(5, p);
  status.setJointPosition(6, w);

  printf("MoveToType(%d) %f %f %f / %f %f %f\n",
	 cmd->CommandID->val,
	 x, y, z, r, p, w);

  return 0;
}
Пример #14
0
void osgParticleHPS::ParticleTrail::renderDebug( float scale ) const
{
    osg::Matrix rotMtx;
    osg::Vec3 ptA, ptB;
    osg::Vec3 xaxis( 1., 0., 0. );
    osg::Vec3 yaxis( 0., 1., 0. );
    osg::Vec3 zaxis( 0., 0., 1. );

    glBegin( GL_LINES );
    int i;
    int numPoints = positions_.size();
    for( i = 0; i < numPoints; i++ )
    {
        const HPSTrailPoint &point = positions_[i];
        float sTexCoord = (float)i / (float)numPoints;
        rotMtx.makeRotate(
            point.rpy[0], yaxis,
            point.rpy[1], xaxis,
            point.rpy[2], zaxis
        );

        ptA = osg::Vec3(scale, 0., 0.) * rotMtx;
        ptA += point.pos;

        glVertex3fv( point.pos.ptr() );
        glVertex3fv( ptA.ptr() );

        ptA = osg::Vec3(0., 0., scale) * rotMtx;
        ptA += point.pos;

        glVertex3fv( point.pos.ptr() );
        glVertex3fv( ptA.ptr() );
    }
    glEnd();

}
Пример #15
0
void osgParticleHPS::ParticleTrail::renderCrossRibbon() const
{
    osg::Matrix rotMtx;
    osg::Vec3 ptA, ptB;
    osg::Vec3 xaxis( 1., 0., 0. );
    osg::Vec3 yaxis( 0., 1., 0. );
    osg::Vec3 zaxis( 0., 0., 1. );
    int numPoints = positions_.size();

    float t = 0.;
    float tIncrement = 1. / maxNumPointsToRecord_;
    const osgParticleHPS::rangev4 &colorRange = getColorRange();
    const osgParticleHPS::Interpolator *colorInterp = getColorInterpolator();
    const osgParticleHPS::rangev3 &sizeRange = getSizeRange();
    const osgParticleHPS::Interpolator *sizeInterp = getSizeInterpolator();

    // This offset will give the ribbon texture a much smoother feel.
    // Without it, the texture would "pulse" and jump as the points at the
    // tail end of the ribbon are removed.
    float sTexCoordOffset = 1. / (float)numPoints;
    sTexCoordOffset *= (t0_ - timestampForLastNewPoint_) / timeBetweenPoints_;

    glBegin( GL_QUAD_STRIP );
    int i;
    for( i = 0; i < numPoints; i++ )
    {
        const HPSTrailPoint &point = positions_[i];
        osg::Vec4 interpColor = colorInterp->interpolate( t, colorRange );
        osg::Vec3 thisPosSize = sizeInterp->interpolate( t, sizeRange );
        t += tIncrement;
        float sTexCoord;

        if( stretchTexture )
        {
            sTexCoord = (float)i / (float)numPoints;
            if( i > 0 )
                sTexCoord += sTexCoordOffset;
        }
        else
        {
            if( totalHistoryPoints%2 )
                sTexCoord = (i%2) ? 0.0 : 1.0;
            else
                sTexCoord = (i%2) ? 1.0 : 0.0;
        }

        rotMtx.makeRotate(
            point.rpy[0], yaxis,
            point.rpy[1], xaxis,
            point.rpy[2], zaxis
        );
        ptA = osg::Vec3(thisPosSize.x(), 0., 0.) * rotMtx;
        ptB = ptA * -1.;
        ptA += point.pos;
        ptB += point.pos;

        glColor4f( interpColor[0], interpColor[1], interpColor[2], interpColor[3] );

        glTexCoord2f( sTexCoord, 1. );
        glVertex3fv( ptA.ptr() );

        glTexCoord2f( sTexCoord, 0. );
        glVertex3fv( ptB.ptr() );

    }
    glEnd();

    t = 0.0;

    glBegin( GL_QUAD_STRIP );
    for( i = 0; i < numPoints; i++ )
    {
        const HPSTrailPoint &point = positions_[i];
        osg::Vec4 interpColor = colorInterp->interpolate( t, colorRange );
        osg::Vec3 thisPosSize = sizeInterp->interpolate( t, sizeRange );
        t += tIncrement;
        float sTexCoord;

        if( stretchTexture )
        {
            sTexCoord = (float)i / (float)numPoints;
            if( i > 0 )
                sTexCoord += sTexCoordOffset;
        }
        else
        {
            if( totalHistoryPoints%2 )
                sTexCoord = (i%2) ? 0.0 : 1.0;
            else
                sTexCoord = (i%2) ? 1.0 : 0.0;
        }

        rotMtx.makeRotate(
            point.rpy[0], yaxis,
            point.rpy[1], xaxis,
            point.rpy[2], zaxis
        );
        ptA = osg::Vec3(0., 0., thisPosSize.z()) * rotMtx;
        ptB = ptA * -1.;
        ptA += point.pos;
        ptB += point.pos;

        glColor4f( interpColor[0], interpColor[1], interpColor[2], interpColor[3] );

        glTexCoord2f( sTexCoord, 1. );
        glVertex3fv( ptA.ptr() );

        glTexCoord2f( sTexCoord, 0. );
        glVertex3fv( ptB.ptr() );

    }

    glEnd();
}
Пример #16
0
//--------------------------------------------------------------
// render the planet
void Planet::render(
  int graphicsWidth,
  int graphicsHeight,
  double angle)
{
  // if shader didn't compile, nothing we can do
  if (m_planetShader == 0)
    return;

  double atmos = 20.0;
  double radius = 1200.0;

  double eyeDist = m_eyePt.length()/radius;
  double a = 1.0;
  if (eyeDist < a)
    return;  // below surface, nothing to do 
  double b = sqrt(eyeDist*eyeDist - a*a);
  double h = (a*b)/eyeDist;
  double m = (a*a)/eyeDist;

  h += atmos/radius;

  // x axis from planet center towards eye
  mgPoint3 xaxis(m_eyePt);
  xaxis.normalize();

  // build y axis
  mgPoint3 yaxis(xaxis);
  yaxis.cross(mgPoint3(0.0, 1.0, 0.0));
  yaxis.normalize();

  mgPoint3 zaxis(yaxis);
  zaxis.cross(xaxis);
  zaxis.normalize();

  mgMatrix4 transform;
  transform._11 = xaxis.x;
  transform._12 = xaxis.y;
  transform._13 = xaxis.z;
  transform._21 = yaxis.x;
  transform._22 = yaxis.y;
  transform._23 = yaxis.z;
  transform._31 = zaxis.x;
  transform._32 = zaxis.y;
  transform._33 = zaxis.z;
  
  VertexPlanet tl, tr, bl, br;

  mgPoint3 pt;
  transform.mapPt(m, -h, h, pt.x, pt.y, pt.z);
  tl.setPoint(radius*pt.x, radius*pt.y, radius*pt.z);

  transform.mapPt(m, h, h, pt.x, pt.y, pt.z);
  tr.setPoint(radius*pt.x, radius*pt.y, radius*pt.z);

  transform.mapPt(m, -h, -h, pt.x, pt.y, pt.z);
  bl.setPoint(radius*pt.x, radius*pt.y, radius*pt.z);

  transform.mapPt(m, h, -h, pt.x, pt.y, pt.z);
  br.setPoint(radius*pt.x, radius*pt.y, radius*pt.z);

  // inverse of world transform
  mgMatrix4 model;
  model.rotateYDeg(-angle);

  mgPoint3 lightDir(1.0, 0.25, 0.0);
  lightDir.normalize();

  mgPoint3 modelLightDir;
  model.mapPt(lightDir, modelLightDir);
  transform.multiply(model);

  mgPoint3 modelEye; 
  transform.mapPt(eyeDist, 0.0, 0.0, modelEye.x, modelEye.y, modelEye.z);

  transform.mapPt(m, -h, h, pt.x, pt.y, pt.z);
  tl.setModelPoint(pt.x, pt.y, pt.z);

  transform.mapPt(m, h, h, pt.x, pt.y, pt.z);
  tr.setModelPoint(pt.x, pt.y, pt.z);

  transform.mapPt(m, -h, -h, pt.x, pt.y, pt.z);
  bl.setModelPoint(pt.x, pt.y, pt.z);

  transform.mapPt(m, h, -h, pt.x, pt.y, pt.z);
  br.setModelPoint(pt.x, pt.y, pt.z);

  mgMatrix4 viewMatrix;
  viewMatrix.translate(-m_eyePt.x, -m_eyePt.y, -m_eyePt.z);
  viewMatrix.multiply(m_eyeMatrix);

  mgMatrix4 worldProjection;
  createProjection(worldProjection, graphicsWidth, graphicsHeight);

  mgMatrix4 mvpMatrix(viewMatrix);
  mvpMatrix.multiply(worldProjection);

  setupShader(mvpMatrix, modelEye, modelLightDir);

  glActiveTexture(GL_TEXTURE0);
  glBindTexture(GL_TEXTURE_CUBE_MAP, m_surfaceTexture); 
  glActiveTexture(GL_TEXTURE1);
  glBindTexture(GL_TEXTURE_CUBE_MAP, m_cloudsTexture); 

  glBindVertexArray(m_vertexArray);
  glBindBuffer(GL_ARRAY_BUFFER, m_vertexBuffer);

  VertexPlanet data[6];
  data[0] = tl;
  data[1] = tr;
  data[2] = bl;
  data[3] = bl;
  data[4] = tr;
  data[5] = br;
  int vertexSize = sizeof(VertexPlanet);
  int count = 6;
  glBufferData(GL_ARRAY_BUFFER, vertexSize * count, data, GL_DYNAMIC_DRAW);

  glDrawArrays(GL_TRIANGLES, 0, count);

  glBindBuffer(GL_ARRAY_BUFFER, 0);
  glBindVertexArray(0);
  glActiveTexture(GL_TEXTURE0);
}
Пример #17
0
coVR1DTransInteractor::coVR1DTransInteractor(pfVec3 o, pfVec3 d, float min, float max, float val, float size, int showConstr, char *name)
{
   pfMatrix wm, tm, sm;
   pfVec3 zaxis(0,0,1);

   origin = o;
   dir = d;
   dir.normalize();
   minVal = min;
   maxVal = max;
   currVal = val;
   interSize = size;
   interactorName= new char[strlen(name)+1];
   strcpy(interactorName, name);
   if ((showConstr == SHOW_CONSTRAINTS_ALWAYS)
      || (showConstr == SHOW_CONSTRAINTS_ONTOUCH)
      || (showConstr == SHOW_CONSTRAINTS_ONSELECT))
      showConstraints = showConstr;

   else
   {
      fprintf(stderr,"\nERROR in coVR1DTransInteractor::coVR1DTransInteractor\n");
      fprintf(stderr,"\tillegal value [%d] for showConstraints\n", showConstr);

      showConstraints = SHOW_CONSTRAINTS_ALWAYS;
   }
   float interScale = size/cover->getScale();
   fprintf(stderr,"interactor size=%f cover scale = %f\n", interSize, cover->getScale());
   // initialize flags

   interactionOngoing = false;
   isI = false;
   isS = false;
   isE = false;
   justHit = true;

   // get debug level
   const char *line = CoviseConfig::getEntry("COVERConfig.DEBUG_LEVEL");
   if (line)
      sscanf(line,"%d", &debugLevel);
   else
      debugLevel = 0;

   // debugprint
   if (debugLevel>0)
   {
      fprintf(stderr,"\ncoVR1DTransInteractor:coVR1DTransInteractor [%s]\n", interactorName);
      fprintf(stderr,"\tshowConstraints=[%d]\n", showConstraints);
      fprintf(stderr,"\torigin=[%f %f %f]\n", origin[0], origin[1], origin[2]);
      fprintf(stderr,"\tdir=[%f %f %f]\n", dir[0], dir[1], dir[2]);
      fprintf(stderr,"\tminVal=[%f] maxVal=[%f] currVal=[%f]\n", minVal, maxVal, currVal);

   }

   // check and adjust the ranges
   checkRange();

   // load geostates only once
   geostate = loadDefaultGeostate();
   unlightedGeostate = loadUnlightedGeostate();

   //       objectsRoot
   //            |
   //        worldDCS
   //            |
   //       interactorRoot
   //          |   |
   // lineGeode   sphereTransDCS
   //                  |
   //             sphereScaleDCS
   //                  |
   //             sphereGeode

   if(debugLevel>1)
      fprintf(stderr,"\tcreating the scenegraph\n");

   objectsRoot = cover->getObjectsRoot();

   worldDCS = new pfDCS();
   wm.makeVecRotVec(zaxis, dir);
   wm[3][0] = origin[0];
   wm[3][1] = origin[1];
   wm[3][2] = origin[2];
   worldDCS->setMat(wm);

   interactorRoot = new pfGroup();

   sphereTransDCS = new pfDCS();
   tm.makeTrans(0, 0, currVal);
   sphereTransDCS->setMat(tm);

   sphereScaleDCS = new pfDCS();
   sm.makeScale(interScale, interScale, interScale);
   sphereScaleDCS->setMat(sm);

   sphereGeode = createSphere();
   lineGeode = createLine();

   objectsRoot->addChild(worldDCS);
   worldDCS->addChild(interactorRoot);
   interactorRoot->addChild(lineGeode);
   interactorRoot->addChild(sphereTransDCS);
   sphereTransDCS->addChild(sphereScaleDCS);
   sphereScaleDCS->addChild(sphereGeode);

   if (showConstraints == SHOW_CONSTRAINTS_ALWAYS)
      showLine();
   else
      hideLine();

   if(debugLevel>1)
      fprintf(stderr,"\tcreating the highlights\n");

   // highlights
   isectedHl = new pfHighlight();
   isectedHl->setMode(PFHL_FILL);
   isectedHl->setColor(PFHL_FGCOLOR, 0.6, 0.6, 0.0);
   isectedHl->setLineWidth(3.0);

   selectedHl = new pfHighlight();
   selectedHl->setMode(PFHL_FILL);
   selectedHl->setColor(PFHL_FGCOLOR, 0.0, 0.6, 0.0);
   selectedHl->setLineWidth(3.0);

}
Пример #18
0
//--------------------------------------------------------------
// get avatar orientation under coordinate system
void SolarSystem::getCoordBasis(
  int coordSystem,
  mgMatrix4& basis)
{
  // get position of moon
  double angle = (2*PI*m_moonMonth)/360;
  mgPoint3 moonCenter(MOON_DISTANCE*sin(angle), 0, MOON_DISTANCE*cos(angle));

  // get coordinate system based on object
  mgPoint3 xaxis(1.0, 0.0, 0.0);
  mgPoint3 yaxis(0.0, 1.0, 0.0);
  mgPoint3 zaxis(0.0, 0.0, 1.0);

  switch (coordSystem)
  {
    case COORDS_PLANET:
      // y axis points away from center of planet
      yaxis = m_coordPosn;
      yaxis.normalize();

      // construct z axis orthonal to y
      zaxis = yaxis;
      zaxis.cross(mgPoint3(0.0, 1.0, 0.0));
      zaxis.normalize();

      // construct x axis
      xaxis = yaxis;
      xaxis.cross(zaxis);
      xaxis.normalize();
      break;

    case COORDS_MOON:
      // y axis points away from center of moon
      yaxis = m_coordPosn;
      yaxis.normalize();

      // construct z axis orthonal to y
      zaxis = yaxis;
      zaxis.cross(mgPoint3(0.0, 1.0, 0.0));
      zaxis.normalize();

      // construct x axis
      xaxis = yaxis;
      xaxis.cross(zaxis);
      xaxis.normalize();
      break;

    case COORDS_RING:
      // y axis points towards center of ring
      yaxis.x = -m_coordPosn.x;
      yaxis.z = -m_coordPosn.z;
      yaxis.y = 0.0;
      yaxis.normalize();

      // x axis is across the ring
      xaxis.x = 0.0; xaxis.y = 1.0; xaxis.z = 0.0;

      // construct z axis
      zaxis = xaxis;
      zaxis.cross(yaxis);
      zaxis.normalize();
      break;

    case COORDS_BELT:
      break;

    case COORDS_SPACE:
      // use default axis
      break;
  }

  // apply object surface orientation
  basis._11 = xaxis.x;
  basis._21 = xaxis.y;
  basis._31 = xaxis.z;
  basis._12 = yaxis.x;
  basis._22 = yaxis.y;
  basis._32 = yaxis.z;
  basis._13 = zaxis.x;
  basis._23 = zaxis.y;
  basis._33 = zaxis.z;
}
bool OBBRayPicking::testRayOBBIntersection(
	glm::vec3 ray_origin,        // Ray origin, in world space
	glm::vec3 ray_direction,     // Ray direction (NOT target position!), in world space. Must be normalize()'d.
	glm::vec3 aabb_min,          // Minimum X,Y,Z coords of the mesh when not transformed at all.
	glm::vec3 aabb_max,          // Maximum X,Y,Z coords. Often aabb_min*-1 if your mesh is centered, but it's not always the case.
	const glm::mat4 &ModelMatrix,       // Transformation applied to the mesh (which will thus be also applied to its bounding box)
	const glm::vec3 &position, // the position because model matrix doesnt do anything in this game "right now"
	float &intersection_distance // Output : distance between ray_origin and the intersection with the OBB
	){

	// Intersection method from Real-Time Rendering and Essential Mathematics for Games

	float tMin = 0.0f;
	float tMax = 100000.0f;

	glm::vec3 OBBposition_worldspace(ModelMatrix[3].x * position.x, ModelMatrix[3].y * position.y, ModelMatrix[3].z * position.z);

	glm::vec3 delta = OBBposition_worldspace - ray_origin;

	// Test intersection with the 2 planes perpendicular to the OBB's X axis
	{
		glm::vec3 xaxis(ModelMatrix[0].x, ModelMatrix[0].y, ModelMatrix[0].z);
		float e = glm::dot(xaxis, delta);
		float f = glm::dot(ray_direction, xaxis);
		
		if (fabs(f) > 0.001f)
		{ // Standard case

			float t1 = (e + aabb_min.x) / f; // Intersection with the "left" plane
			float t2 = (e + aabb_max.x) / f; // Intersection with the "right" plane
			// t1 and t2 now contain distances betwen ray origin and ray-plane intersections

			// We want t1 to represent the nearest intersection, 
			// so if it's not the case, invert t1 and t2
			if (t1>t2){
				float w = t1; t1 = t2; t2 = w; // swap t1 and t2
			}

			// tMax is the nearest "far" intersection (amongst the X,Y and Z planes pairs)
			if (t2 < tMax)
				tMax = t2;
			// tMin is the farthest "near" intersection (amongst the X,Y and Z planes pairs)
			if (t1 > tMin)
				tMin = t1;

			// And here's the trick :
			// If "far" is closer than "near", then there is NO intersection.
			// See the images in the tutorials for the visual explanation.
			if (tMax < tMin)
				return false;

		}
		else
		{ // Rare case : the ray is almost parallel to the planes, so they don't have any "intersection"
			if (-e + aabb_min.x > 0.0f || -e + aabb_max.x < 0.0f)
				return false;
		}
	}


	// Test intersection with the 2 planes perpendicular to the OBB's Y axis
	// Exactly the same thing than above.
	{
		glm::vec3 yaxis(ModelMatrix[1].x, ModelMatrix[1].y, ModelMatrix[1].z);
		float e = glm::dot(yaxis, delta);
		float f = glm::dot(ray_direction, yaxis);

		if (fabs(f) > 0.001f){

			float t1 = (e + aabb_min.y) / f;
			float t2 = (e + aabb_max.y) / f;

			if (t1>t2){ float w = t1; t1 = t2; t2 = w; }

			if (t2 < tMax)
				tMax = t2;
			if (t1 > tMin)
				tMin = t1;
			if (tMin > tMax)
				return false;

		}
		else{
			if (-e + aabb_min.y > 0.0f || -e + aabb_max.y < 0.0f)
				return false;
		}
	}


	// Test intersection with the 2 planes perpendicular to the OBB's Z axis
	// Exactly the same thing than above.
	{
		glm::vec3 zaxis(ModelMatrix[2].x, ModelMatrix[2].y, ModelMatrix[2].z);
		float e = glm::dot(zaxis, delta);
		float f = glm::dot(ray_direction, zaxis);

		if (fabs(f) > 0.001f){

			float t1 = (e + aabb_min.z) / f;
			float t2 = (e + aabb_max.z) / f;

			if (t1>t2){ float w = t1; t1 = t2; t2 = w; }

			if (t2 < tMax)
				tMax = t2;
			if (t1 > tMin)
				tMin = t1;
			if (tMin > tMax)
				return false;

		}
		else{
			if (-e + aabb_min.z > 0.0f || -e + aabb_max.z < 0.0f)
				return false;
		}
	}

	intersection_distance = tMin;
	return true;

}
Пример #20
0
RagDoll::RagDoll (btDynamicsWorld* ownerWorld, const btVector3& positionOffset, 
				  btScalar scale_ragdoll)	: m_ownerWorld (ownerWorld) {

	// setup colors
	for (int i = 0; i < BODYPART_COUNT; i++) {
//		ofVec4f col = ofVec4f(ofRandom(0.7, 1.0),
//								ofRandom(0.5, 1.0),
//								ofRandom(0.7, 1.0),
//								1.0);
		ofVec4f col = ofVec4f(0.7,
								0.7,
								0.7,
								1.0);		
		colors.push_back(col);
	}

	// Setup the geometry
	m_shapes[BODYPART_PELVIS] = new btCapsuleShape(btScalar(scale_ragdoll*0.15), btScalar(scale_ragdoll*0.20));
	m_shapes[BODYPART_SPINE] = new btCapsuleShape(btScalar(scale_ragdoll*0.15), btScalar(scale_ragdoll*0.28));
	m_shapes[BODYPART_HEAD] = new btCapsuleShape(btScalar(scale_ragdoll*0.10), btScalar(scale_ragdoll*0.05));
	m_shapes[BODYPART_LEFT_UPPER_LEG] = new btCapsuleShape(btScalar(scale_ragdoll*0.07), btScalar(scale_ragdoll*0.45));
	m_shapes[BODYPART_LEFT_LOWER_LEG] = new btCapsuleShape(btScalar(scale_ragdoll*0.05), btScalar(scale_ragdoll*0.37));
	m_shapes[BODYPART_RIGHT_UPPER_LEG] = new btCapsuleShape(btScalar(scale_ragdoll*0.07), btScalar(scale_ragdoll*0.45));
	m_shapes[BODYPART_RIGHT_LOWER_LEG] = new btCapsuleShape(btScalar(scale_ragdoll*0.05), btScalar(scale_ragdoll*0.37));
	m_shapes[BODYPART_LEFT_UPPER_ARM] = new btCapsuleShape(btScalar(scale_ragdoll*0.05), btScalar(scale_ragdoll*0.33));
	m_shapes[BODYPART_LEFT_LOWER_ARM] = new btCapsuleShape(btScalar(scale_ragdoll*0.04), btScalar(scale_ragdoll*0.25));
	m_shapes[BODYPART_RIGHT_UPPER_ARM] = new btCapsuleShape(btScalar(scale_ragdoll*0.05), btScalar(scale_ragdoll*0.33));
	m_shapes[BODYPART_RIGHT_LOWER_ARM] = new btCapsuleShape(btScalar(scale_ragdoll*0.04), btScalar(scale_ragdoll*0.25));

	// Setup all the rigid bodies
	btTransform offset; offset.setIdentity();
	offset.setOrigin(positionOffset);
	
	// fix the rotation
	double angle = (double)180*(TWO_PI/360.0);
	btVector3 zaxis(0.0,0.0,1.0);
	btQuaternion zquat(zaxis,angle);
	btVector3 yaxis(0.0,1.0,0.0);
	btQuaternion yquat(yaxis,angle);
	btQuaternion quat = zquat*yquat;	
	offset.setRotation(quat);	

	btTransform transform;
	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(0.), btScalar(scale_ragdoll*1.), btScalar(0.)));
	m_bodies[BODYPART_PELVIS] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_PELVIS]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(0.), btScalar(scale_ragdoll*1.2), btScalar(0.)));
	m_bodies[BODYPART_SPINE] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_SPINE]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(0.), btScalar(scale_ragdoll*1.6), btScalar(0.)));
	m_bodies[BODYPART_HEAD] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_HEAD]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(-0.18*scale_ragdoll), btScalar(0.65*scale_ragdoll),
btScalar(0.)));
	m_bodies[BODYPART_LEFT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_LEG]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(-0.18*scale_ragdoll), btScalar(0.2*scale_ragdoll), btScalar(0.)));
	m_bodies[BODYPART_LEFT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_LEG]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(0.18*scale_ragdoll), btScalar(0.65*scale_ragdoll), btScalar(0.)));
	m_bodies[BODYPART_RIGHT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_LEG]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(0.18*scale_ragdoll), btScalar(0.2*scale_ragdoll), btScalar(0.)));
	m_bodies[BODYPART_RIGHT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_LEG]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(-0.35*scale_ragdoll), btScalar(1.45*scale_ragdoll), btScalar(0.)));
	transform.getBasis().setEulerZYX(0,0,SIMD_HALF_PI);
	m_bodies[BODYPART_LEFT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_ARM]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(-0.7*scale_ragdoll), btScalar(1.45*scale_ragdoll), btScalar(0.)));
	transform.getBasis().setEulerZYX(0,0,SIMD_HALF_PI);
	m_bodies[BODYPART_LEFT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_ARM]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(0.35*scale_ragdoll), btScalar(1.45*scale_ragdoll), btScalar(0.)));
	transform.getBasis().setEulerZYX(0,0,-SIMD_HALF_PI);
	m_bodies[BODYPART_RIGHT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_ARM]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(0.7*scale_ragdoll), btScalar(1.45*scale_ragdoll), btScalar(0.)));
	transform.getBasis().setEulerZYX(0,0,-SIMD_HALF_PI);
	m_bodies[BODYPART_RIGHT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_ARM]);

	// Setup some damping on the m_bodies
	for (int i = 0; i < BODYPART_COUNT; ++i)
	{
		m_bodies[i]->setDamping(0.05, 0.85);
		m_bodies[i]->setDeactivationTime(0.8);
		m_bodies[i]->setSleepingThresholds(1.6, 2.5);
	}

///////////////////////////// SETTING THE CONSTRAINTS /////////////////////////////////////////////7777
	// Now setup the constraints
	btGeneric6DofConstraint * joint6DOF;
	btTransform localA, localB;
	bool useLinearReferenceFrameA = true;
/// ******* SPINE HEAD ******** ///
	{
		localA.setIdentity(); localB.setIdentity();

		localA.setOrigin(btVector3(btScalar(0.), btScalar(0.30*scale_ragdoll), btScalar(0.)));

		localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14*scale_ragdoll), btScalar(0.)));

		joint6DOF = new btGeneric6DofConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_HEAD], localA, localB,useLinearReferenceFrameA);

#ifdef RIGID
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_PI*0.3f,-SIMD_EPSILON,-SIMD_PI*0.3f));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.5f,SIMD_EPSILON,SIMD_PI*0.3f));
#endif
		m_joints[JOINT_SPINE_HEAD] = joint6DOF;
		m_ownerWorld->addConstraint(m_joints[JOINT_SPINE_HEAD], true);
	}
/// *************************** ///




/// ******* LEFT SHOULDER ******** ///
	{
		localA.setIdentity(); localB.setIdentity();

		localA.setOrigin(btVector3(btScalar(-0.2*scale_ragdoll), btScalar(0.15*scale_ragdoll), btScalar(0.)));

		localB.getBasis().setEulerZYX(SIMD_HALF_PI,0,-SIMD_HALF_PI);
		localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18*scale_ragdoll), btScalar(0.)));

		joint6DOF = new btGeneric6DofConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB,useLinearReferenceFrameA);

#ifdef RIGID
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_PI*0.8f,-SIMD_EPSILON,-SIMD_PI*0.5f));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.8f,SIMD_EPSILON,SIMD_PI*0.5f));
#endif
		m_joints[JOINT_LEFT_SHOULDER] = joint6DOF;
		m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_SHOULDER], true);
	}
/// *************************** ///


/// ******* RIGHT SHOULDER ******** ///
	{
		localA.setIdentity(); localB.setIdentity();

		localA.setOrigin(btVector3(btScalar(0.2*scale_ragdoll), btScalar(0.15*scale_ragdoll), btScalar(0.)));
		localB.getBasis().setEulerZYX(0,0,SIMD_HALF_PI);
		localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18*scale_ragdoll), btScalar(0.)));
		joint6DOF = new btGeneric6DofConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB,useLinearReferenceFrameA);

#ifdef RIGID
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else

		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_PI*0.8f,-SIMD_EPSILON,-SIMD_PI*0.5f));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.8f,SIMD_EPSILON,SIMD_PI*0.5f));
#endif
		m_joints[JOINT_RIGHT_SHOULDER] = joint6DOF;
		m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_SHOULDER], true);
	}
/// *************************** ///

/// ******* LEFT ELBOW ******** ///
	{
		localA.setIdentity(); localB.setIdentity();

		localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18*scale_ragdoll), btScalar(0.)));
		localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14*scale_ragdoll), btScalar(0.)));
		joint6DOF =  new btGeneric6DofConstraint (*m_bodies[BODYPART_LEFT_UPPER_ARM], *m_bodies[BODYPART_LEFT_LOWER_ARM], localA, localB,useLinearReferenceFrameA);

#ifdef RIGID
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.7,SIMD_EPSILON,SIMD_EPSILON));
#endif
		m_joints[JOINT_LEFT_ELBOW] = joint6DOF;
		m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_ELBOW], true);
	}
/// *************************** ///

/// ******* RIGHT ELBOW ******** ///
	{
		localA.setIdentity(); localB.setIdentity();

		localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18*scale_ragdoll), btScalar(0.)));
		localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14*scale_ragdoll), btScalar(0.)));
		joint6DOF =  new btGeneric6DofConstraint (*m_bodies[BODYPART_RIGHT_UPPER_ARM], *m_bodies[BODYPART_RIGHT_LOWER_ARM], localA, localB,useLinearReferenceFrameA);

#ifdef RIGID
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.7,SIMD_EPSILON,SIMD_EPSILON));
#endif

		m_joints[JOINT_RIGHT_ELBOW] = joint6DOF;
		m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_ELBOW], true);
	}
/// *************************** ///


/// ******* PELVIS ******** ///
	{
		localA.setIdentity(); localB.setIdentity();

		localA.getBasis().setEulerZYX(0,SIMD_HALF_PI,0);
		localA.setOrigin(btVector3(btScalar(0.), btScalar(0.15*scale_ragdoll), btScalar(0.)));
		localB.getBasis().setEulerZYX(0,SIMD_HALF_PI,0);
		localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.15*scale_ragdoll), btScalar(0.)));
		joint6DOF =  new btGeneric6DofConstraint (*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_SPINE], localA, localB,useLinearReferenceFrameA);

#ifdef RIGID
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_PI*0.2,-SIMD_EPSILON,-SIMD_PI*0.3));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.2,SIMD_EPSILON,SIMD_PI*0.6));
#endif
		m_joints[JOINT_PELVIS_SPINE] = joint6DOF;
		m_ownerWorld->addConstraint(m_joints[JOINT_PELVIS_SPINE], true);
	}
/// *************************** ///

/// ******* LEFT HIP ******** ///
	{
		localA.setIdentity(); localB.setIdentity();

		localA.setOrigin(btVector3(btScalar(-0.18*scale_ragdoll), btScalar(-0.10*scale_ragdoll), btScalar(0.)));

		localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225*scale_ragdoll), btScalar(0.)));

		joint6DOF = new btGeneric6DofConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_LEFT_UPPER_LEG], localA, localB,useLinearReferenceFrameA);

#ifdef RIGID
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_HALF_PI*0.5,-SIMD_EPSILON,-SIMD_EPSILON));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_HALF_PI*0.8,SIMD_EPSILON,SIMD_HALF_PI*0.6f));
#endif
		m_joints[JOINT_LEFT_HIP] = joint6DOF;
		m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_HIP], true);
	}
/// *************************** ///


/// ******* RIGHT HIP ******** ///
	{
		localA.setIdentity(); localB.setIdentity();

		localA.setOrigin(btVector3(btScalar(0.18*scale_ragdoll), btScalar(-0.10*scale_ragdoll), btScalar(0.)));
		localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225*scale_ragdoll), btScalar(0.)));

		joint6DOF = new btGeneric6DofConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB,useLinearReferenceFrameA);

#ifdef RIGID
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_HALF_PI*0.5,-SIMD_EPSILON,-SIMD_HALF_PI*0.6f));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_HALF_PI*0.8,SIMD_EPSILON,SIMD_EPSILON));
#endif
		m_joints[JOINT_RIGHT_HIP] = joint6DOF;
		m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_HIP], true);
	}
/// *************************** ///


/// ******* LEFT KNEE ******** ///
	{
		localA.setIdentity(); localB.setIdentity();

		localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225*scale_ragdoll), btScalar(0.)));
		localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185*scale_ragdoll), btScalar(0.)));
		joint6DOF =  new btGeneric6DofConstraint (*m_bodies[BODYPART_LEFT_UPPER_LEG], *m_bodies[BODYPART_LEFT_LOWER_LEG], localA, localB,useLinearReferenceFrameA);
//
#ifdef RIGID
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.7f,SIMD_EPSILON,SIMD_EPSILON));
#endif
		m_joints[JOINT_LEFT_KNEE] = joint6DOF;
		m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_KNEE], true);
	}
/// *************************** ///

/// ******* RIGHT KNEE ******** ///
	{
		localA.setIdentity(); localB.setIdentity();

		localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225*scale_ragdoll), btScalar(0.)));
		localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185*scale_ragdoll), btScalar(0.)));
		joint6DOF =  new btGeneric6DofConstraint (*m_bodies[BODYPART_RIGHT_UPPER_LEG], *m_bodies[BODYPART_RIGHT_LOWER_LEG], localA, localB,useLinearReferenceFrameA);

#ifdef RIGID
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
#else
		joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
		joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI*0.7f,SIMD_EPSILON,SIMD_EPSILON));
#endif
		m_joints[JOINT_RIGHT_KNEE] = joint6DOF;
		m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_KNEE], true);
	}
/// *************************** ///

}
Пример #21
0
bool MPUtil::RayOBBIntersection(
	DCCore::Point_3  vA,        // Ray origin, in world space
	DCCore::Point_3  vB,     // Ray direction (NOT target position!), in world space. Must be normalize()'d.
	DCCore::Point_3  aabb_min,          // Minimum X,Y,Z coords of the mesh when not transformed at all.
	DCCore::Point_3  aabb_max,          // Maximum X,Y,Z coords. Often aabb_min*-1 if your mesh is centered, but it's not always the case.
	const double* ModelMatrix,       // Transformation applied to the mesh (which will thus be also applied to its bounding box)
	float& intersection_distance // Output : distance between ray_origin and the intersection with the OBB
	)
{
	DCCore::Point_3  ray_origin = vA;      // Ray origin, in world space
	DCCore::Point_3  ray_direction ; 
	DCCore::Point_3 temp = (vB-vA);

	temp.Normalize();
	ray_direction = temp;
	// Intersection method from Real-Time Rendering and Essential Mathematics for Games

	float tMin = 0.0f;
	float tMax = 100000.0f;

	DCCore::Point_3  OBBposition_worldspace(0, 0, 0);

	DCCore::Point_3 delta = OBBposition_worldspace - ray_origin;

	// Test intersection with the 2 planes perpendicular to the OBB's X axis
	{
		DCCore::Point_3 xaxis(1, 0, 0);
		float e = xaxis.Dot(delta);
		float f = ray_direction.Dot(xaxis);

		if ( fabs(f) > 0.001f ){ // Standard case

			float t1 = (e+aabb_min.x)/f; // Intersection with the "left" plane
			float t2 = (e+aabb_max.x)/f; // Intersection with the "right" plane
			// t1 and t2 now contain distances betwen ray origin and ray-plane intersections

			// We want t1 to represent the nearest intersection, 
			// so if it's not the case, invert t1 and t2
			if (t1>t2){
				float w=t1;t1=t2;t2=w; // swap t1 and t2
			}

			// tMax is the nearest "far" intersection (amongst the X,Y and Z planes pairs)
			if ( t2 < tMax )
				tMax = t2;
			// tMin is the farthest "near" intersection (amongst the X,Y and Z planes pairs)
			if ( t1 > tMin )
				tMin = t1;

			// And here's the trick :
			// If "far" is closer than "near", then there is NO intersection.
			// See the images in the tutorials for the visual explanation.
			if (tMax < tMin )
				return false;

		}else{ // Rare case : the ray is almost parallel to the planes, so they don't have any "intersection"
			if(-e+aabb_min.x > 0.0f || -e+aabb_max.x < 0.0f)
				return false;
		}
	}


	// Test intersection with the 2 planes perpendicular to the OBB's Y axis
	// Exactly the same thing than above.
	{
		DCCore::Point_3 yaxis(0, 1, 0);
		float e = yaxis.Dot(delta);
		float f = ray_direction.Dot(yaxis);

		if ( fabs(f) > 0.001f ){

			float t1 = (e+aabb_min.y)/f;
			float t2 = (e+aabb_max.y)/f;

			if (t1>t2){float w=t1;t1=t2;t2=w;}

			if ( t2 < tMax )
				tMax = t2;
			if ( t1 > tMin )
				tMin = t1;
			if (tMin > tMax)
				return false;

		}else{
			if(-e+aabb_min.y > 0.0f || -e+aabb_max.y < 0.0f)
				return false;
		}
	}


	// Test intersection with the 2 planes perpendicular to the OBB's Z axis
	// Exactly the same thing than above.
	{
		DCCore::Point_3 zaxis(0, 0, 1);
		float e = zaxis.Dot(delta);
		float f = ray_direction.Dot(zaxis);

		if ( fabs(f) > 0.001f ){

			float t1 = (e+aabb_min.z)/f;
			float t2 = (e+aabb_max.z)/f;

			if (t1>t2){float w=t1;t1=t2;t2=w;}

			if ( t2 < tMax )
				tMax = t2;
			if ( t1 > tMin )
				tMin = t1;
			if (tMin > tMax)
				return false;

		}else{
			if(-e+aabb_min.z > 0.0f || -e+aabb_max.z < 0.0f)
				return false;
		}
	}

	intersection_distance = tMin;
	return true;

}
/*! The mouseMove is called by the viewer when the mouse is moved in the
    viewer and this handle is the active one.

    \param x the x-pos of the mouse (pixel)
    \param y the y-pos of the mouse (pixel)
 */
void PlaneMoveManipulator::mouseMove(const Int16 x,
                            const Int16 y)
{
    SLOG << "==============================" << endLog;
    SLOG << "PlaneMoveManipulator::mouseMove() enter x=" << x << " y=" << y << endLog;

    // get the beacon's core (must be ComponentTransform) and it's center
    if( getTarget() == NULL )
    {
        SWARNING << "Handle has no target.\n";
        return;
        
    }
    // get transformation of beacon
    Transform *t = dynamic_cast<Transform *>(getTarget()->getCore());

    if( t == NULL )
    {
        SWARNING << "handled object has no parent transform!\n";
        return;
    }

    Vec3f      translation;       // for matrix decomposition
    Quaternion rotation;
    Vec3f      scaleFactor;
    Quaternion scaleOrientation;

    t->getMatrix().getTransform(translation, rotation, scaleFactor,
                                scaleOrientation);

    OSG::Line viewray;
    getViewport()->getCamera()->calcViewRay(viewray, x, y, *getViewport());

    SLOG << "Manipulator::mouseMove(): viewray: " << viewray << endLog;

    // Get manipulator axes into world space
    OSG::Matrix tm = getTarget()->getToWorld();
    
    Vec3f rot_axis;
    tm.multFull(Vec3f(0,1,0), rot_axis);

    Plane pl(rot_axis, getClickPoint());

    Pnt3f plpoint;
    
    if (pl.intersect(viewray, plpoint) == true) // Ignore moving out of the plane...
    {
        SLOG << "Manipulator::mouseMove(): plpoint: " << plpoint << endLog;
         
        Vec3f      trans = getBaseTranslation();
        Quaternion rot   = getBaseRotation();

        // Get manipulator axes into world space
        Vec3f xp,zp;

        tm.multFull(Vec3f(1,0,0), xp);
        tm.multFull(Vec3f(0,0,1), zp);
        
        if (getActiveSubHandle() == getHandleXNode())
        {
            Line xaxis(getClickPoint(), xp);
            Line zaxis(getClickPoint(), zp);

            Real32 fx = xaxis.getClosestPointT(plpoint);
            Real32 fz = zaxis.getClosestPointT(plpoint);

            SLOG << "Manipulator::mouseMove(): xaxis: " << xaxis << " zaxis: " << zaxis <<endLog;
            SLOG << "Manipulator::mouseMove(): fx: " << fx << " fz: " << fz <<endLog;
        
            // Alternative: transform hitpoint into manip space
            OSG::Matrix m = getTarget()->getToWorld();
            m.invert();
            
            Pnt3f mpoint;
            m.mult(plpoint, mpoint);
            
            SLOG << "Manipulator::mouseMove(): mpoint:" << mpoint << endLog;

            trans = trans + xp * fx + zp * fz;
        }
        else if (getActiveSubHandle() == getHandleZNode())
        {
            Pnt3f wcenter;
            
            tm.multFull(Pnt3f(0,getLength()[1],0), wcenter);
            
            Vec3f vclick, vcurrent;
            
            vclick = getClickPoint() - wcenter;
            vcurrent = plpoint - wcenter;
            
            vclick.normalize();
            vcurrent.normalize();
            
            Real32 a = vclick.enclosedAngle(vcurrent);
            
            SLOG << "Manipulator::mouseMove(): wcenter:" << wcenter << "" <<endLog;
            SLOG << "Manipulator::mouseMove(): vclick:" << vclick << " vcurrent:" << vcurrent <<endLog;
            SLOG << "Manipulator::mouseMove(): angle:" << a << " deg: " << osgRad2Degree(a) << endLog;
        }
        
        Matrix m;

        m.setTransform(trans, rot, scaleFactor, scaleOrientation);

        t->setMatrix(m);
        
    }

    setLastMousePos(Pnt2f(Real32(x), Real32(y)));
    updateHandleTransform();

    //SLOG << "Manipulator::mouseMove() leave\n" << std::flush;
}
Пример #23
0
bool Matrix::decompose(Vector3* scale, Quaternion* rotation, Vector3* translation) const
{
    if (translation)
    {
        // Extract the translation.
        translation->x = m[12];
        translation->y = m[13];
        translation->z = m[14];
    }

    // Nothing left to do.
    if (scale == NULL && rotation == NULL)
        return true;

    // Extract the scale.
    // This is simply the length of each axis (row/column) in the matrix.
    Vector3 xaxis(m[0], m[1], m[2]);
    float scaleX = xaxis.length();

    Vector3 yaxis(m[4], m[5], m[6]);
    float scaleY = yaxis.length();

    Vector3 zaxis(m[8], m[9], m[10]);
    float scaleZ = zaxis.length();

    // Determine if we have a negative scale (true if determinant is less than zero).
    // In this case, we simply negate a single axis of the scale.
    float det = determinant();
    if (det < 0)
        scaleZ = -scaleZ;

    if (scale)
    {
        scale->x = scaleX;
        scale->y = scaleY;
        scale->z = scaleZ;
    }

    // Nothing left to do.
    if (rotation == NULL)
        return true;

    // Scale too close to zero, can't decompose rotation.
    if (scaleX < MATH_TOLERANCE || scaleY < MATH_TOLERANCE || fabs(scaleZ) < MATH_TOLERANCE)
        return false;

    float rn;

    // Factor the scale out of the matrix axes.
    rn = 1.0f / scaleX;
    xaxis.x *= rn;
    xaxis.y *= rn;
    xaxis.z *= rn;

    rn = 1.0f / scaleY;
    yaxis.x *= rn;
    yaxis.y *= rn;
    yaxis.z *= rn;

    rn = 1.0f / scaleZ;
    zaxis.x *= rn;
    zaxis.y *= rn;
    zaxis.z *= rn;

    // Now calculate the rotation from the resulting matrix (axes).
    float trace = xaxis.x + yaxis.y + zaxis.z + 1.0f;

    if (trace > MATH_EPSILON)
    {
        float s = 0.5f / sqrt(trace);
        rotation->w = 0.25f / s;
        rotation->x = (yaxis.z - zaxis.y) * s;
        rotation->y = (zaxis.x - xaxis.z) * s;
        rotation->z = (xaxis.y - yaxis.x) * s;
    }
    else
    {
        // Note: since xaxis, yaxis, and zaxis are normalized, 
        // we will never divide by zero in the code below.
        if (xaxis.x > yaxis.y && xaxis.x > zaxis.z)
        {
            float s = 0.5f / sqrt(1.0f + xaxis.x - yaxis.y - zaxis.z);
            rotation->w = (yaxis.z - zaxis.y) * s;
            rotation->x = 0.25f / s;
            rotation->y = (yaxis.x + xaxis.y) * s;
            rotation->z = (zaxis.x + xaxis.z) * s;
        }
        else if (yaxis.y > zaxis.z)
        {
            float s = 0.5f / sqrt(1.0f + yaxis.y - xaxis.x - zaxis.z);
            rotation->w = (zaxis.x - xaxis.z) * s;
            rotation->x = (yaxis.x + xaxis.y) * s;
            rotation->y = 0.25f / s;
            rotation->z = (zaxis.y + yaxis.z) * s;
        }
        else
        {
            float s = 0.5f / sqrt(1.0f + zaxis.z - xaxis.x - yaxis.y );
            rotation->w = (xaxis.y - yaxis.x ) * s;
            rotation->x = (zaxis.x + xaxis.z ) * s;
            rotation->y = (zaxis.y + yaxis.z ) * s;
            rotation->z = 0.25f / s;
        }
    }

    return true;
}
Пример #24
0
BZ_END_STENCIL


/*
 * Allocate arrays and set their initial state
 */
void setup(const int N, vectorField& V, vectorField& nextV, scalarField& P,
    scalarField& P_rhs, vectorField& advect, vectorField& force)
{
    // A 1m x 1m x 1m domain
    spatialStep = 1.0 / (N - 1);
    geom = UniformCubicGeometry<3>(spatialStep);

    // Allocate arrays
    allocateArrays(shape(N,N,N), advect, V, nextV, force);  // vector fields
    allocateArrays(shape(N,N,N), P, P_rhs);                 // scalar fields

    // Since incompressibility is assumed, pressure only shows up as
    // derivative terms in the equations.  We choose airPressure = 0
    // as an arbitrary datum.

    airPressure = 0;             // Pa
    rho = 1000;                  // density of fluid, kg/m^3
    recip_rho = 1.0 / rho;       // inverse of density
    eta = 1.0e-6;                // kinematic viscosity of fluid, m^2/s
    gravity = 9.81;              // m/s^2
    delta_t = 0.001;             // initial time step, in seconds
    volume = pow3(spatialStep);  // cubic volume associated with grid point

    // Kludge: Set eta high, so that the flow will spread faster.
    // This means the cube is filled with molasses, rather than water.
    eta *= 1000;
   
    // Initial conditions: quiescent
    V = 0.0; 
    P_rhs = 0.0;
    advect = 0.0;
    nextV = 0.0;

    // Initial pressure condition: gravity causes a linear increase
    // in pressure with depth.  Note that tensor::k means the index
    // associated with the z axis (they are labelled i, j, k).
#ifdef NO_GRAVITY
    gravityPressureGradient = 0.0;
    P = 0.0;
#else
    gravityPressureGradient = spatialStep * gravity * rho;

#ifdef BZ_HAVE_NAMESPACES
    P = airPressure + tensor::k * gravityPressureGradient;
#else
    P = airPressure + k * gravityPressureGradient;
#endif

#endif

    // Set up the forcing function: gravity plus a stirring force
    // at the bottom
    double gravity_z = gravity * rho;

    const int x = 0, y = 1, z = 2;
    force[x] = 0.0;
    force[y] = 0.0;
#ifdef NO_GRAVITY
    force[z] = 0.0;
#else
    force[z] = gravity_z;    
#endif

#ifndef NO_STIRRING
    // Centre of the stirring
    int centrex = int(2 * N / 3.0);
    int centrey = int(2 * N / 3.0);
    int centrez = int(4 * N / 5.0);

    const double stirRadius = 1.0 / 3.0;

    vector3d zaxis(0,0,1);

    // Loop through the 2D slice where the stirring occurs

    for (int i=force.lbound(firstDim); i <= force.ubound(firstDim); ++i)
    {
      for (int j=force.lbound(secondDim); j <= force.ubound(secondDim); ++j)
      {
         // Vector from the centre of the stirring to the current
         // coordinate

         vector3d r((i-centrex) * spatialStep, (j-centrey) * spatialStep, 0.0);

         if (norm(r) < stirRadius)
         {
             // The cross product of the z-axis and the vector3d to this
             // coordinate yields the direction of the force.  Multiply
             // by gravity to get a reasonable magnitude (max force =
             // 5 * gravity)
             force(i,j,centrez) += cross(zaxis, r) 
                 * (5 * gravity_z / stirRadius);
         }
      }
    }
#endif // NO_STIRRING
}