//-----------------------------------------------------------------------
	void MeshRotationAffector::_affectParticles(ParticleSystem* pSystem, Real timeElapsed)
	{
		ParticleIterator pi = pSystem->_getIterator();
		Particle *p;
		Real ds;

		// Rotation adjustments by time
		ds = timeElapsed;

		while (!pi.end())
		{
			p = pi.getNext();

			MeshParticleVisualData *data = static_cast<MeshParticleVisualData *>(p->getVisualData());

			data->mYawRotation = data->mYawRotation + ds * data->mYawRotationSpeed;

			data->mPitchRotation = data->mPitchRotation + ds * data->mPitchRotationSpeed;

			data->mRollRotation = data->mRollRotation + ds * data->mRollRotationSpeed;

			if ( data->mYawRotation != Radian(0) || data->mPitchRotation != Radian(0) || 
				data->mRollRotation != Radian(0) )
				pSystem->_notifyParticleRotated();
		}

	}
예제 #2
0
	//---------------------------------------------------------------------
	void Overlay::updateTransform(void) const
	{
		// Ordering:
		//    1. Scale
		//    2. Rotate
		//    3. Translate

		Radian orientationRotation = Radian(0);

#if OGRE_NO_VIEWPORT_ORIENTATIONMODE == 0
		orientationRotation = Radian(OverlayManager::getSingleton().getViewportOrientationMode() * Math::HALF_PI);
#endif

		Matrix3 rot3x3, scale3x3;
		rot3x3.FromEulerAnglesXYZ(Radian(0), Radian(0), mRotate + orientationRotation);
		scale3x3 = Matrix3::ZERO;
		scale3x3[0][0] = mScaleX;
		scale3x3[1][1] = mScaleY;
		scale3x3[2][2] = 1.0f;

		mTransform = Matrix4::IDENTITY;
		mTransform = rot3x3 * scale3x3;
		mTransform.setTrans(Vector3(mScrollX, mScrollY, 0));

		mTransformOutOfDate = false;
	}
void LightAndObjectsManager::attachConLight(Vector3 pos, Vector3 direction, IDrawable *parent)
{
	SceneNode *lnode = 
		sceneNodeMgr->getNode(parent)->createChildSceneNode();

    std::stringstream out;
    out << lnode->getName();
    out << "-" << pos.x << "-" << pos.y << "-" << pos.z;

    string lightS = "-conLight";
    lightS += out.str();

    Light* light = sceneManager->createLight(lightS);
    light->setType(Light::LT_SPOTLIGHT);
    light->setDiffuseColour(1.0,1.0,1.0);
    light->setSpecularColour(1.0,1.0,1.0);
    //light->setAttenuation( 100, 1.0, 0.045, 0.0075);
    light->setSpotlightInnerAngle(Radian(Degree(5)));
    light->setSpotlightOuterAngle(Radian(Degree(100)));
    light->setSpotlightFalloff(40.0);
    light->setDirection(direction);
    //light->setSpotlightRange(Ogre::Degree(20), Ogre::Degree(60), 1.2);
    //light->setDirection(Vector3::NEGATIVE_UNIT_Y);
    
    lnode->attachObject(light);
    
    lnode->setPosition(pos);
}
예제 #4
0
	//-----------------------------------------------------------------------
	Radian Quaternion::getYaw(bool reprojectAxis) const
	{
		if (reprojectAxis)
		{
			// yaw = atan2(localz.x, localz.z)
			//拾取部分 zAxis() 实现我们需要
			Real fTx = 2.0f*x;
			Real fTy = 2.0f*y;
			Real fTz = 2.0f*z;
			Real fTwy = fTy*w;
			Real fTxx = fTx*x;
			Real fTxz = fTz*x;
			Real fTyy = fTy*y;

			// Vector3(fTxz+fTwy, fTyz-fTwx, 1.0-(fTxx+fTyy));

			return Radian(Math::ATan2(fTxz + fTwy, 1.0f - (fTxx + fTyy)));

		}
		else
		{
			// 实现我们需要
			return Radian(Math::ASin(-2 * (x*z - w*y)));
		}
	}
예제 #5
0
    //-----------------------------------------------------------------------
	Radian Quaternion::getYaw(bool reprojectAxis) const
	{
		if (reprojectAxis)
		{
			// yaw = atan2(localz.x, localz.z)
			// pick parts of zAxis() implementation that we need
			Real fTx  = 2.0f*x;
			Real fTy  = 2.0f*y;
			Real fTz  = 2.0f*z;
			Real fTwy = fTy*w;
			Real fTxx = fTx*x;
			Real fTxz = fTz*x;
			Real fTyy = fTy*y;

			// Vector3(fTxz+fTwy, fTyz-fTwx, 1.0-(fTxx+fTyy));

			return Radian(Math::ATan2(fTxz+fTwy, 1.0f-(fTxx+fTyy)));

		}
		else
		{
			// internal version
			return Radian(Math::ASin(-2*(x*z - w*y)));
		}
	}
예제 #6
0
void RotationGizmo::update()
{
    mArrowNode->setScale( Gizmo::getSceneNode()->getScale() );
    Ogre::Radian rfAngle = Ogre::Radian( (mDirector.x * (Real)mMouse->mMouseState.x.rel) - 
        (mDirector.y * (Real)mMouse->mMouseState.y.rel) ) * 0.02;
    
    if( Gizmo::isSnappingToGrid() )
    {
        mRotationAccumulator += toRadian<Radian>( rfAngle );

        if( mRotationAccumulator.valueDegrees() >= 45 )
        {
            Gizmo::getControlledObject()->rotate( toVector3<Vector3>( mRotationAxis ), 
                Radian( Degree( 45 ) ), Node::TS_LOCAL );
            mRotationAccumulator = 0;
        }
        else if( mRotationAccumulator.valueDegrees() <= -45 )
        {
            Gizmo::getControlledObject()->rotate( toVector3<Vector3>( mRotationAxis ), 
                Radian( Degree( -45 ) ), Node::TS_LOCAL );
            mRotationAccumulator = 0;
        }
    }
    else
    {
        Gizmo::getControlledObject()->rotate( toVector3<Vector3>( mRotationAxis ), 
            toRadian<Radian>( rfAngle ), Node::TS_LOCAL );
    }

    mMouse->mMouseState.clear();
}
예제 #7
0
//-----------------------------------------------------------------------
bool Matrix3::ToEulerAnglesZYX(Radian& rfYAngle, Radian& rfPAngle,
                               Radian& rfRAngle) const
{
	// rot =  cy*cz           cz*sx*sy-cx*sz  cx*cz*sy+sx*sz
	//        cy*sz           cx*cz+sx*sy*sz -cz*sx+cx*sy*sz
	//       -sy              cy*sx           cx*cy
	rfPAngle = Math::ASin(-m[2][0]);

	if (rfPAngle < Radian(Math::HALF_PI))
	{
		if (rfPAngle > Radian(-Math::HALF_PI))
		{
			rfYAngle = Math::ATan2(m[1][0], m[0][0]);
			rfRAngle = Math::ATan2(m[2][1], m[2][2]);
			return true;
		}
		else
		{
			// WARNING.  Not a unique solution.
			Radian fRmY = Math::ATan2(-m[0][1], m[0][2]);
			rfRAngle = Radian(0.0);  // any angle works
			rfYAngle = rfRAngle - fRmY;
			return false;
		}
	}
	else
	{
		// WARNING.  Not a unique solution.
		Radian fRpY = Math::ATan2(-m[0][1], m[0][2]);
		rfRAngle = Radian(0.0);  // any angle works
		rfYAngle = fRpY - rfRAngle;
		return false;
	}
}
예제 #8
0
bool Camera::OnSpecialKeyboard(int Key) {
    bool Ret = false;

    switch(Key) {
        case GLUT_KEY_UP:
        {
            pitch(Radian(Degree(5)));
            Ret = true;
        }
        break;

    case GLUT_KEY_DOWN:
        {
            pitch(Radian(Degree(-5)));
            Ret = true;
        }
        break;

    case GLUT_KEY_LEFT:
        {
            yaw(Radian(Degree(-5)));
            Ret = true;
        }
        break;

    case GLUT_KEY_RIGHT:
        {
            yaw(Radian(Degree(5)));
            Ret = true;
        }
        break;
    }

    return Ret;
}
예제 #9
0
void GearsMeshBuilder::rotate( scalar rotX,scalar rotY,scalar rotZ )
{
	Matrix3 mat;
	mat.FromEulerAnglesXYZ(Radian(rotX),Radian(rotY),Radian(rotZ));
	Quaternion quat(mat);

	{
		MeshSkeletonVector::Iterator i;
		for (i=mMySkeletons.Begin(); i!=mMySkeletons.End(); ++i)
		{
			MeshSkeleton *ms = (*i);
			for (int32 j=0; j<ms->mBoneCount; j++)
			{
				MeshBone &b = ms->mBones[j];
				if ( b.mParentIndex == -1 )
				{
					b.mPosition = quat * b.mPosition;
					b.mOrientation = quat * b.mOrientation;
				}
			}
		}
	}

	{
		GearMeshVector::Iterator i;
		for (i=mMyMeshes.Begin(); i!=mMyMeshes.End(); ++i)
		{
			GearMesh *m = (*i);
			uint32 vcount = m->mVertexPool.GetSize();
			if ( vcount > 0 )
			{
				MeshVertex *vb = m->mVertexPool.GetBuffer();
				for (uint32 j=0; j<vcount; j++)
				{
					vb->mPos = quat * vb->mPos;
					vb++;
				}
			}
		}
	}

	{
		MeshAnimationVector::Iterator i;
		for (i=mMyAnimations.Begin(); i!=mMyAnimations.End(); ++i)
		{
			MeshAnimation *ma = (*i);
			for (int32 j=0; j<ma->mTrackCount && j <1; j++)
			{
				MeshAnimTrack *t = ma->mTracks[j];
				for (int32 k=0; k<t->mFrameCount; k++)
				{
					MeshAnimPose &p = t->mPose[k];
					p.mPos = quat * p.mPos;
					p.mQuat = quat * p.mQuat;
				}
			}
		}
	}
}
예제 #10
0
 Ogre::Vector3 MathUtil::sphericalToCartesian(Ogre::Real r,
     Ogre::Radian azimuth, Ogre::Radian altitude)
 {
     Vector3 rval;
     rval.x = r*Math::Sin(azimuth)*Math::Sin(altitude + Radian(Math::HALF_PI));
     rval.y = r*Math::Cos(altitude + Radian(Math::HALF_PI));
     rval.z = r*Math::Cos(azimuth)*Math::Sin(altitude + Radian(Math::HALF_PI));
     return rval;
 }
예제 #11
0
PanGestureDetector::AngleThresholdPair PanGestureDetector::GetAngle(size_t index) const
{
  PanGestureDetector::AngleThresholdPair ret( Radian(0),Radian(0) );

  if( index < mAngleContainer.size() )
  {
    ret = mAngleContainer[index];
  }

  return ret;
}
	//-----------------------------------------------------------------------
	TextureRotator::TextureRotator(void) : 
		ParticleAffector(),
		mUseOwnRotationSpeed(DEFAULT_USE_OWN_SPEED),
		mScaledRotationSpeed(Radian(0)),
		twoPiRad(Radian(Math::TWO_PI))
	{
		mDynRotation = PU_NEW_T(DynamicAttributeFixed, MEMCATEGORY_SCENE_OBJECTS)();
		(static_cast<DynamicAttributeFixed*>(mDynRotation))->setValue(DEFAULT_ROTATION);
		mDynRotationSpeed = PU_NEW_T(DynamicAttributeFixed, MEMCATEGORY_SCENE_OBJECTS)();
		(static_cast<DynamicAttributeFixed*>(mDynRotationSpeed))->setValue(DEFAULT_ROTATION_SPEED);
	}
예제 #13
0
	Quaternion Vector3::GetRotationTo(Vector3 dest, Vector3 fallbackAxis)
    {
        // Based on Stan Melax's article in Game Programming Gems
        Quaternion q;
        // Copy, since cannot modify local
        Vector3 v0 = *this;
        Vector3 v1 = dest;
        v0.Normalise();
        v1.Normalise();

        Real d = v0.DotProduct(v1);
        // If dot == 1, vectors are the same
        if (d >= 1.0f)
        {
            return Quaternion::IDENTITY;
        }
		
		// sometimes the dot product yields -1.0000001
		// floating point math does that to you
		if (d < -1.0f)
			d = -1.0f;

        Real s = Math::Sqrt( (1+d)*2 );
		if (s < 1e-6f)
		{
			if (fallbackAxis != Vector3::ZERO)
			{
				// rotate 180 degrees about the fallback axis
				q.FromAngleAxis(Radian(Math::PI), fallbackAxis);
			}
			else
			{
				// Generate an axis
				Vector3 axis = Vector3::UNIT_X.CrossProduct(*this);
				if (axis.IsZeroLength) // pick another if colinear
					axis = Vector3::UNIT_Y.CrossProduct(*this);
				axis.Normalise();
				q.FromAngleAxis(Radian(Math::PI), axis);
			}
		}
		else
		{
            Real invs = 1 / s;

			Vector3 c = v0.CrossProduct(v1);

	        q.x = c.x * invs;
    	    q.y = c.y * invs;
        	q.z = c.z * invs;
        	q.w = s * 0.5;
			q.Normalise();
		}
        return q;
    }
	//-----------------------------------------------------------------------
	void VortexAffector::_preProcessParticles(ParticleTechnique* particleTechnique, Real timeElapsed)
	{
		ParticleSystem* sys = mParentTechnique->getParentSystem();
		if (sys)
		{
			mRotation.FromAngleAxis(Radian(_calculateRotationSpeed() * timeElapsed), sys->getDerivedOrientation() * mRotationVector);
		}
		else
		{
			mRotation.FromAngleAxis(Radian(_calculateRotationSpeed() * timeElapsed), mRotationVector);
		}
		getDerivedPosition();
	}
예제 #15
0
 Radian Math::acos(float val)
 {
     if (-1.0f < val)
     {
         if (val < 1.0f)
             return Radian(std::acos(val));
         else
             return Radian(0.0f);
     }
     else
     {
         return Radian(PI);
     }
 }
 //-----------------------------------------------------------------------
 Radian Math::ASin (Real fValue)
 {
     if ( -1.0 < fValue )
     {
         if ( fValue < 1.0 )
             return Radian(asin(fValue));
         else
             return Radian(HALF_PI);
     }
     else
     {
         return Radian(-HALF_PI);
     }
 }
예제 #17
0
Radian ASin( scalar fValue )
{
	if ( -1.0 < fValue )
	{
		if ( fValue < 1.0 )
			return Radian(asin(fValue));
		else
			return Radian(HALF_PI);
	}
	else
	{
		return Radian(-HALF_PI);
	}
}
예제 #18
0
Radian ACos( scalar fValue )
{
	if ( -1.0 < fValue )
	{
		if ( fValue < 1.0 )
			return Radian(acos(fValue));
		else
			return Radian(0.0);
	}
	else
	{
		return Radian(PI);
	}
}
 //-----------------------------------------------------------------------
 Radian Math::ACos (Real fValue)
 {
     if ( -1.0 < fValue )
     {
         if ( fValue < 1.0 )
             return Radian(acos(fValue));
         else
             return Radian(0.0);
     }
     else
     {
         return Radian(PI);
     }
 }
예제 #20
0
 Radian Math::asin(float val)
 {
     if (-1.0f < val)
     {
         if (val < 1.0f)
             return Radian(std::asin(val));
         else
             return Radian(HALF_PI);
     }
     else
     {
         return Radian(-HALF_PI);
     }
 }
예제 #21
0
// -----------------------------------------------------------------------------------------
CPepeEngineQuaternion CPepeEngineVector3::getRotationTo(
    const CPepeEngineVector3& dest,
    const CPepeEngineVector3& fallbackAxis) const
{
    // Based on Stan Melax's article in Game Programming Gems
    CPepeEngineQuaternion q;
    // Copy, since cannot modify local
    CPepeEngineVector3 v0 = *this;
    CPepeEngineVector3 v1 = dest;
    v0.normalise();
    v1.normalise();

    float d = v0.dotProduct(v1);

    // If dot == 1, vectors are the same
    if (d >= 1.0f) {
        return CPepeEngineQuaternion::IDENTITY;
    }

    if (d < (1e-6f - 1.0f)) {
        if (fallbackAxis != CPepeEngineVector3::ZERO) {
            // rotate 180 degrees about the fallback axis
            q.fromAngleAxis(Radian(CPepeEngineMath::PI), fallbackAxis);
        } else {
            // Generate an axis
            CPepeEngineVector3 axis = CPepeEngineVector3::UNIT_X.crossProduct(*this);

            if (axis.isZeroLength()) { // pick another if colinear
                axis = CPepeEngineVector3::UNIT_Y.crossProduct(*this);
            }

            axis.normalise();
            q.fromAngleAxis(Radian(CPepeEngineMath::PI), axis);
        }
    } else {
        float s = CPepeEngineMath::Sqrt((1 + d) * 2);
        float invs = 1 / s;

        CPepeEngineVector3 c = v0.crossProduct(v1);

        q.x = c.x * invs;
        q.y = c.y * invs;
        q.z = c.z * invs;
        q.w = s * 0.5f;
        q.normalise();
    }

    return q;
}
예제 #22
0
    //-----------------------------------------------------------------------
    void TerrainCircleLineList::build( TerrainManager* terrainMgr , Vec3 center, Flt radius, Flt gap )
    {
        Flt     girth           = 2 * 3.14 * radius;
        UInt    sectorCount     = girth / gap;
        UInt    maxSectorCount  = 100;
        if ( sectorCount > maxSectorCount )
            sectorCount = maxSectorCount;
        if ( sectorCount < 6 )
            sectorCount = 6;

        Vector3 dir         = Vector3( 0, 0, radius );
        Flt onceRadianValue = 2 * 3.14 / sectorCount;
        Radian onceRadian   = Radian( onceRadianValue );

        //////////////////////////////////////////////////////

        Ogre::Quaternion baseQua;

        Flt delta       = MGTimeOp::getCurrTick() - mLastBuildTime;
        mLastBuildTime  = MGTimeOp::getCurrTick();
        Flt speed       = 0.003;
        mBaseRadian += delta * speed * onceRadianValue;
        if ( mBaseRadian > 2 * 3.14 )
            mBaseRadian = 0;

        baseQua.FromAngleAxis( Radian(mBaseRadian), Vector3::UNIT_Y);
        dir = baseQua * dir;

        //////////////////////////////////////////////////////

        Quaternion q;
        q.FromAngleAxis( onceRadian, Vector3::UNIT_Y);

        //////////////////////////////////////////////////////
        mCircleLine.clear();
        Vec3 pos;
        for ( UInt i=0; i<sectorCount; i++ )
        {
            pos = center + Vec3(dir.x,dir.y,dir.z);

            //////////////////////////////////////////////////////////////////
            
            terrainMgr->getStickHeight( Vec2(pos.x, pos.z), pos.y );
            
            //////////////////////////////////////////////////////////////////
            mCircleLine.push_back( pos );
            dir = q * dir;
        }
    }
예제 #23
0
// Move the whirler
void Whirler::move(Real delta) {
	// Rotate
	mNode->roll(Radian(-10 * delta));
	mNode->pitch(Radian(mRotationSpeed * delta));
	mFlareNode->roll(Radian(delta * 0.25f));

	Vector3 pos = getPosition();

	// Try to avoid all moving objects
	ObjectMapType::const_iterator i;
	ObjectSystem *ob = ObjectSystem::getSingletonPtr();
	for(i=ob->getFirst(); i != ob->getLast(); ++i) {
		if((*i).second == this || (*i).second->getType() == OTYPE_MUSHROOM) continue;

		Vector3 d = pos - (*i).second->getPosition();
		d.z = 0;
		if(d.squaredLength() > 60.0f*60.0f) continue;
		Real dist = d.normalise();
		Real effect = 1.0f - (dist / 60.0f);

		mCurrentSpeed += d * effect * mSpeed * 3.0f * delta;
	}

	// ..and player as well
	Vector3 d = pos - mPlayer->getPosition();
	d.z = 0;
	if(d.squaredLength() <= 60.0f*60.0f) {
		Real dist = d.normalise();
		// If distance is very small, player catches it
		if(dist < 3.0f) {
			pickUp();
			setToBeDeleted();
			mPlayer->catchWhirler();
			return;
		}
		Real effect = 1.0f - (dist / 60.0f);
		mCurrentSpeed += d * effect * mSpeed * 10.0f * delta;
	}

	// Cap the speed
	if(mCurrentSpeed.squaredLength() >= 55*55) {
		mCurrentSpeed = mCurrentSpeed.normalisedCopy() * 55.0f;
	}

	// Move
	mNode->translate(mCurrentSpeed * delta);
	mFlareNode->setPosition(mNode->getPosition());
}
예제 #24
0
	inline Quaternion slerp(const Quaternion x, const Quaternion y, f32 t)
	{
		Quaternion z = y;

		f32 cosTheta = dot(x, y);

		if(cosTheta < 0.0f)
		{
			z = -y;
			cosTheta = -cosTheta;
		}

		Quaternion result;

		if(cosTheta > 1.0f)
		{
			result = Quaternion{ lerp(x.x, y.x, t),
								lerp(x.y, y.y, t),
								lerp(x.z, y.z, t),
								lerp(x.w, y.w, t) };

			return result;
		}

		Radian angle = Math::acos(cosTheta);

		result = Math::sin(Radian(1.0f) - (t * angle)) * x + Math::sin(t * angle) * z;
		result = result * (1.0f / Math::sin(angle));

		return result;
	}
예제 #25
0
	Camera::Camera(const String& name, SceneManager* sm)
		:Frustum(name),
		mSceneMgr(sm),
		mOrientation(Quaternion::IDENTITY),
		mPosition(Vector3::ZERO),
		mSceneDetail(PM_SOLID),
		mLastViewport(NULL),
		mAutoAspectRatio(FALSE),
		mPixelDisplayRatio(0)
	{
		mFOVy = Radian(Math::PI / 4.0f);
		mNearDist = 100.0f;
		mFarDist = 100000.0f;
		mAspect = 1.333333333333f;
		mProjType = PT_PERSPECTIVE;
		setFixedYawAxis(TRUE);
		invalidateFrustum();
		invalidateView();
		mViewMatrix = Matrix4::ZERO;
		mProjMatrixRS = Matrix4::ZERO;
		mParentNode = NULL;
		mReflect = FALSE;
		mVisible = FALSE;
		
	}
예제 #26
0
 void MathUtil::cartesianToSpherical(Ogre::Vector3 cartesian, Ogre::Real& r,
     Ogre::Radian& azimuth, Ogre::Radian& altitude)
 {
     r = Math::Sqrt(Math::Sqr(cartesian.x)*Math::Sqr(cartesian.y)*Math::Sqr(cartesian.z));
     azimuth = Math::ATan2(cartesian.x, cartesian.z);
     altitude = Math::ACos(cartesian.y/r) - Radian(Math::HALF_PI);
 }
예제 #27
0
// Ball worm constructor
BallWorm::BallWorm(Real orbitRad, Real rotSpeed, Real angleStep, Player *player, const String &name, SceneManager *sceneMgr, const Vector3 &pos, bool managed) : Asteroid(name, sceneMgr, "", "", pos, managed) {
	mType = OTYPE_BALLWORM;
	mRotationSpeed = rotSpeed;
	mCurrentSpeed.x = 0; //Math::RangeRandom(-10, 10);
	mCurrentSpeed.y = 0; //Math::RangeRandom(-10, 10);
	mCurrentSpeed.z = 0;
	mPlayer = player;

	mOrbitRadius = orbitRad;
	mRotationAxis = Vector3::UNIT_Z;

	// Create the balls
	for(int f=0; f<numBalls; f++) {
		mBallDestroyed[f] = false;
		mBalls[f] = mNode->createChildSceneNode(mName + "BallNode" + StringConverter::toString(f));
		mBalls[f]->translate(mOrbitRadius, 0, 0, SceneNode::TS_WORLD);

		Entity *ent = mSceneMgr->createEntity(mName + "BallEntity" + StringConverter::toString(f), "WormBall.mesh");
		ent->setCastShadows(false);
		mBalls[f]->attachObject(ent);
		if(f%2) {
			mBalls[f]->setScale(0.7f, 0.7f, 0.7f);
		}
		mNode->rotate(mRotationAxis, Radian(Degree(angleStep))); // 18.5f
	}
}
예제 #28
0
// -----------------------------------------------------------------------------------------
void CPepeEngineCamera::setDirection(const CPepeEngineVector3& dir)
{
    if (dir == CPepeEngineVector3::ZERO) {
        return;
    }

    /*
        Remember, camera points down -Z of local axes!
        Therefore reverse direction of direction vector before determining local Z
    */

    CPepeEngineVector3 zAdjustVec = -dir;
    zAdjustVec.normalise();

    // Get axes from current quaternion
    CPepeEngineVector3 axes[3];

    m_orientation.toAxes(axes);
    CPepeEngineQuaternion rotQuat;

    if ((axes[2] + zAdjustVec).squaredLength() <  0.00005f) {
        /*
            Oops, a 180 degree turn (infinite possible rotation axes)
            Default to yaw i.e. use current UP
        */
        rotQuat.fromAngleAxis(Radian(CPepeEngineMath::PI), axes[1]);
    } else {
        // Derive shortest arc to new direction
        rotQuat = axes[2].getRotationTo(zAdjustVec);

    }

    m_orientation       = rotQuat * m_orientation;
    m_bNeedViewUpdate   = true;
}
예제 #29
0
void PositionMotor::DoUpdate(const ::gazebo::common::Time &simTime) {
	gz::common::Time stepTime = simTime - prevUpdateTime_;
	if (stepTime <= 0) {
		// Only respond to positive step times
		return;
	}

	prevUpdateTime_ = simTime;
	auto positionAngle = joint_->GetAngle(0);

	// TODO Make sure normalized angle lies within possible range
	// I get the feeling we might be moving motors outside their
	// allowed range. Also something to be aware of when setting
	// the direction.
	positionAngle.Normalize();
	double position = positionAngle.Radian();

	if (fullRange_ && fabs(position - positionTarget_) > M_PI) {
		// Both the position and the position target will be in the range [-pi, pi]
		// For a full range of motion joint, using an angle +- 2 PI might result
		// in a much shorter required movement. In this case we best correct the
		// current position to something outside the range.
		position += (position > 0 ? -2 * M_PI : 2 * M_PI);
	}

	double error = position - positionTarget_;
	double cmd = pid_.Update(error, stepTime);

	joint_->SetParam("vel", 0, cmd);
}
예제 #30
0
 //--------------------------------------------------------------------------
 void AnimableValue::resetToBaseValue(void)
 {
     switch(mType)
     {
     case INT:
         setValue(mBaseValueInt);
         break;
     case REAL:
         setValue(mBaseValueReal[0]);
         break;
     case VECTOR2:
         setValue(Vector2(mBaseValueReal));
         break;
     case VECTOR3:
         setValue(Vector3(mBaseValueReal));
         break;
     case VECTOR4:
         setValue(Vector4(mBaseValueReal));
         break;
     case QUATERNION:
         setValue(Quaternion(mBaseValueReal));
         break;
     case COLOUR:
         setValue(ColourValue(mBaseValueReal[0], mBaseValueReal[1], 
             mBaseValueReal[2], mBaseValueReal[3]));
         break;
     case DEGREE:
         setValue(Degree(mBaseValueReal[0]));
         break;
     case RADIAN:        
         setValue(Radian(mBaseValueReal[0]));
         break;
     }
 }