//----------------------------------------------------------------------- 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(); } }
//--------------------------------------------------------------------- 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); }
//----------------------------------------------------------------------- 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))); } }
//----------------------------------------------------------------------- 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))); } }
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(); }
//----------------------------------------------------------------------- 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; } }
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; }
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; } } } } }
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; }
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); }
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(); }
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); } }
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); } }
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); } }
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); } }
// ----------------------------------------------------------------------------------------- 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; }
//----------------------------------------------------------------------- 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; } }
// 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()); }
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; }
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; }
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); }
// 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 } }
// ----------------------------------------------------------------------------------------- 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; }
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); }
//-------------------------------------------------------------------------- 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; } }