C++ Vector3::angleBetween方法代码示例

bool OgreMesh::intersectTri(const Ogre::Ray &ray, IntersectResult &rtn, Triangle*itr, bool isplane) {
    std::pair<bool, Real> hit = isplane
      ? Ogre::Math::intersects(ray, Ogre::Plane(itr->v1.coord, itr->v2.coord,itr->v3.coord))
      : Ogre::Math::intersects(ray, itr->v1.coord, itr->v2.coord,itr->v3.coord, true, false);
    rtn.u = 0;
    rtn.v = 0;
    if (hit.first && hit.second < rtn.distance) {
      rtn.intersected = hit.first;
      rtn.distance = hit.second;
      Ogre::Vector3 nml=(itr->v1.coord-itr->v2.coord).
              crossProduct(itr->v3.coord-itr->v2.coord);
      rtn.normal.x=nml.x;
      rtn.normal.y=nml.y;
      rtn.normal.z=nml.z;
      rtn.tri = *itr;
      Ogre::Vector3 intersect = ray.getPoint(hit.second) - rtn.tri.v2.coord;
      Ogre::Vector3 aVec = (rtn.tri.v1.coord - rtn.tri.v2.coord);
      Ogre::Vector3 bVec = (rtn.tri.v3.coord - rtn.tri.v2.coord);
      if (aVec.length() > 1.0e-10 && bVec.length() > 1.0e-10) {
        rtn.u = rtn.tri.v2.u + (rtn.tri.v1.u - rtn.tri.v2.u)*cos(aVec.angleBetween(intersect).valueRadians())*intersect.length()/aVec.length();
        rtn.v = rtn.tri.v2.v + (rtn.tri.v3.v - rtn.tri.v2.v)*cos(bVec.angleBetween(intersect).valueRadians())*intersect.length()/bVec.length();
      }
    }
    return rtn.intersected;
}

void EnemyController::approach(void)
{
	EnemyPlane* enemy = static_cast<EnemyPlane*>(IDManager::getPointer(_enemyName, ACTOR));
	Ogre::Vector3 temp = FCKnowledge::getSingleton().getPlayerPosition() - enemy->getPosition();
	Ogre::Vector3 direction = temp * enemy->getAxis();
	//std::cout<<direction.x<<" "<<direction.y<<" "<<direction.z<<std::endl;
	if(direction.angleBetween(Ogre::Vector3::NEGATIVE_UNIT_Z) >= Ogre::Radian(Ogre::Degree(1)))
	{
		Ogre::Quaternion test = direction.getRotationTo(Ogre::Vector3::NEGATIVE_UNIT_Z);
		Ogre::Degree angle = enemy->getRotateLimit();
		
		double yawNum = test.getYaw().valueDegrees()/(angle*WORLD_UPDATE_INTERVAL).valueDegrees();
		yawNum = Ogre::Math::Clamp(yawNum, -1.0, 1.0);
		enemy->yaw(yawNum);

		double pitchNum = test.getPitch().valueDegrees()/(angle*WORLD_UPDATE_INTERVAL).valueDegrees();
		pitchNum = Ogre::Math::Clamp(pitchNum, -1.0, 1.0);
		enemy->pitch(pitchNum);
		
		double rollNum = test.getRoll().valueDegrees()/(angle*WORLD_UPDATE_INTERVAL).valueDegrees();
		rollNum = Ogre::Math::Clamp(rollNum, -1.0, 1.0);
		enemy->roll(rollNum);
		
	}
	else
	{
		enemy->yaw(0);
		enemy->pitch(0);
		enemy->roll(0);
	}
}

    //-----------------------------------------------------------------------
    bool Light::isInLightRange(const Ogre::Sphere& container) const
    {
        bool isIntersect = true;
        //directional light always intersects (check only spotlight and point)
        if (mLightType != LT_DIRECTIONAL)
        {
            //Check that the sphere is within the sphere of the light
            isIntersect = container.intersects(Sphere(mDerivedPosition, mRange));
            //If this is a spotlight, check that the sphere is within the cone of the spot light
            if ((isIntersect) && (mLightType == LT_SPOTLIGHT))
            {
                //check first check of the sphere surrounds the position of the light
                //(this covers the case where the center of the sphere is behind the position of the light
                // something which is not covered in the next test).
                isIntersect = container.intersects(mDerivedPosition);
                //if not test cones
                if (!isIntersect)
                {
                    //Calculate the cone that exists between the sphere and the center position of the light
                    Ogre::Vector3 lightSphereConeDirection = container.getCenter() - mDerivedPosition;
                    Ogre::Radian halfLightSphereConeAngle = Math::ASin(container.getRadius() / lightSphereConeDirection.length());

                    //Check that the light cone and the light-position-to-sphere cone intersect)
                    Radian angleBetweenConeDirections = lightSphereConeDirection.angleBetween(mDerivedDirection);
                    isIntersect = angleBetweenConeDirections <=  halfLightSphereConeAngle + mSpotOuter * 0.5;
                }
            }
        }
        return isIntersect;
    }

void PlayerCharacter::updateCharDirLooking(Ogre::Vector3* aimPoint, Ogre::Vector3* moveDir, unsigned long timeSinceLastFrame)
{
	// Get directions
	Ogre::Vector3 nodeDirection = this->node->getOrientation() * Vector3::UNIT_X;
	Ogre::Vector3 aimDirection = *aimPoint - this->node->getPosition();
	Ogre::Vector3 aimDirectionY = *aimPoint - this->animation->getRightShoulderPosition();

	nodeDirection.y = 0;
	aimDirection.y = 0;
	nodeDirection.normalise();
	aimDirection.normalise();
	aimDirectionY.normalise();

	// Horizontal angle
	Ogre::Radian angleHorizontal = nodeDirection.angleBetween(aimDirection);

	// Is the target on the left or the right side of the direction vector?
	// see http://www.c-plusplus.de/forum/viewtopic-var-t-is-266934.html and http://www.mikrocontroller.net/topic/105993 (German)
	Vector3 A = node->getPosition();					// Stützvektor
	Vector3 B = nodeDirection + node->getPosition();	// Zielpunkt Richtungsvektor (Node Richtung)
	Vector3 C = (*aimPoint);							// Gefragter Punkt
	Vector3 R = B-A;									// Richtungsvektor
	
	if((R.z * (C.x-A.x) - R.x * (C.z - A.z)) > 0)
		angleHorizontal = -angleHorizontal; // Make angle negative if on left side
	angleHorizontal += Ogre::Radian(Ogre::Degree(180)); // From -180° to 180° becomes 0° to 360° (where 180° is forwards)

	// Vertical angle
	Ogre::Radian angleVertical = aimDirection.angleBetween(aimDirectionY);

	if(aimDirectionY.y > 0.f)
		angleVertical = -angleVertical; // Angle negative when looking up
	angleVertical += Ogre::Radian(Ogre::Degree(90)); // From -90° to 90° becomes 0° to 180° (where 0° is up and 180° is down)

	// Use theses angles
	this->animation->updateAimRotation(angleHorizontal, angleVertical, timeSinceLastFrame);	// For Animation
	this->animation->setDirectionVector("nodeDirection",	nodeDirection);					// For AnimationGUI...
	this->animation->setDirectionVector("aimDirHorizontal",	aimDirection);
	this->animation->setDirectionVector("aimDirVertical",	aimDirectionY);
}

//.........这里部分代码省略.........
                                mLaserBeam,
                                currentMeshInformation->vertices[currentMeshInformation->indices[i]],
                                currentMeshInformation->vertices[currentMeshInformation->indices[i+1]],
                                currentMeshInformation->vertices[currentMeshInformation->indices[i+2]],
                                true, // positiveSide
                                false // negativeSide
                                );

                    // if it was a hit, check if its the closest
                    if(hit.first)
                    {
                        if((closestDistanceToEntity < 0.0f) || (hit.second < closestDistanceToEntity))
                        {
                            // this is the closest so far, save it off
                            closestDistanceToEntity = hit.second;
                        }
                    }
                }
            }
        }

        // Do a RSQ against the terrain.
        // http://www.ogre3d.org/docs/api/html/classOgre_1_1TerrainGroup.html says about rayIntersects:
        // This can be called from any thread as long as no parallel write to the terrain data occurs.
        Ogre::TerrainGroup::RayResult rayResultTerrain = mOgreWidget->mTerrainGroup->rayIntersects(mLaserBeam);

        float distanceValidEntity  = 99999.9;
        float distanceValidTerrain = 99999.9;

        if(closestDistanceToEntity > 0)
            distanceValidEntity = closestDistanceToEntity;

        if(rayResultTerrain.hit)
            distanceValidTerrain = mLaserBeam.getOrigin().distance(rayResultTerrain.position);

        float distanceFinal = std::min(distanceValidEntity, distanceValidTerrain);

        // Create gaussian noise around 0, standard deviation increases with distance
        std::normal_distribution<> d(0, 0.005 * qBound(1.0f, distanceFinal, 10.0f));
        distanceFinal += d(*mRandomMersenneTwister);

        if(distanceFinal < mRange)
        {
            const Ogre::Vector3 point = mLaserBeam.getPoint(distanceFinal);

            if(mBottomBeamClockDivisor == 0)
            {
                static Ogre::Vector3 vectorDownWorld = Ogre::Vector3::NEGATIVE_UNIT_Y;
                if(fabs(vectorDownWorld.angleBetween(mLaserBeam.getDirection()).valueDegrees()) < 0.2f)
                {
//                    qDebug() << t() << "LaserScanner::slotDoScan(): currentScanAngle" << mCurrentScanAngle << "emitting heightOverGround, angle is" << vectorDownWorld.angleBetween(mLaserBeam.getDirection()).valueDegrees();
                    emit heightOverGround(distanceFinal);
                }
            }

            mRegisteredPoints << QVector4D(point.x, point.y, point.z, pow(distanceFinal, 2.0));
        }

        // Increase mCurrentScanAngle by mAngleStep for the next laserBeam
        mCurrentScanAngle += mAngleStep;

        gettimeofday(&timeNow, NULL);

        const long long scanTimeElapsed = (timeNow.tv_sec - timeStart.tv_sec) * 1000000 + (timeNow.tv_usec - timeStart.tv_usec);
        const long long scanTimeAtNextRay = realTimeBetweenRaysUS * (numberOfRays+1);

//        if(mNumberOfRaySceneQueries%10000 == 0) qDebug() << "Number of RSQs:" << mNumberOfRaySceneQueries;

        usleep(std::max(0, (int)(scanTimeAtNextRay - scanTimeElapsed)));
    }

    gettimeofday(&timeNow, NULL);

//    const long long timeDiff = (timeNow.tv_sec - timeStart.tv_sec) * 1000000 + (timeNow.tv_usec - timeStart.tv_usec);
//    qDebug() << "LaserScanner::slotDoScan(): took" << timeDiff << "us, should have been" << (long long)(realTimeBetweenRaysUS * ((mAngleStop - mAngleStart)/mAngleStep));

    if(mRegisteredPoints.size())
    {
        emit scanFinished(mSimulator->getSimulationTime());

        //    qDebug() << "LaserScanner::doScan(): emitting" << scanContainer.size() << "points.";

        mScannerPositionQt = QVector3D(mScannerPosition.x, mScannerPosition.y, mScannerPosition.z);

        emit newLidarPoints(mRegisteredPoints, mScannerPositionQt);
    }

    // Set mCurrentScanAngle for the next scan to mAngleStart
    mCurrentScanAngle = mAngleStart;
    mScannerOrientationPrevious = mScannerOrientation;
    mScannerPositionPrevious = mScannerPosition;
    mRegisteredPoints.clear();

    // Sleep for degreesToNextScan = 360.0 - mAngleStop + mAngleStart
    gettimeofday(&timeNow, NULL);
    const long long scanTimeElapsed = (timeNow.tv_sec - timeStart.tv_sec) * 1000000 + (timeNow.tv_usec - timeStart.tv_usec);
    const long long timeRest = (1000000/(mSpeed/60)) * (1.0 / mTimeFactor) - scanTimeElapsed;
//    qDebug() << "LaserScanner::slotDoScan(): emitted results, resting" << timeRest << "us after scan.";
    usleep(std::max(0, (int)timeRest));
}

void Turret::Update()
{
	
	bool PlayerInRoom = false;
	DynamicObject::Update();
	hkpWorldRayCastOutput	OutPut;
	hkpWorldRayCastInput	Ray;
	Player* theplayer = 0;
	Ogre::Vector3 RayDirection = (mPlayerPos - mPosition).normalisedCopy();
	Ray.m_from = hkVector4(mPosition.x + (RayDirection.x * 25)
						,mPosition.y + (RayDirection.y * 25)
						,mPosition.z + (RayDirection.z * 25));
    Ray.m_to = hkVector4(mPlayerPos.x, mPlayerPos.y, mPlayerPos.z);
	mPhysicsManager->GetPhysicsWorld()->castRay(Ray,OutPut);
	if(OutPut.hasHit())
	{
		const hkpCollidable* col = OutPut.m_rootCollidable;
		hkpRigidBody* body = hkpGetRigidBody(col);
		theplayer = dynamic_cast<Player*> ((BaseObject *)body->getUserData());
		if(theplayer != 0)
		{
			PlayerInRoom = true;
		}
		else
		{
			mPlayerInSight = false;
			mKillTimer = 0;
		}
	}
	Ogre::Vector3 NewDir = Ogre::Vector3(RayDirection.x,0,RayDirection.z);
	NewDir.normalise();
	Ogre::Radian angle = NewDir.angleBetween(ObjectNode->getOrientation() * Ogre::Vector3::UNIT_X);
	if(PlayerInRoom || !mPlayerInSight)
	{
		if(angle.valueDegrees() < 20 || angle.valueDegrees() > -20)
		{
			mPlayerInSight = true;
		}
	}
	if(mPlayerInSight)
	{
		mRotateValue += angle.valueRadians();
		Body->setRotation(hkQuaternion(hkVector4(0,1,0),mRotateValue));
		mKillTimer++;
		if(mKillTimer > 800)
		{
			mShutdown = true;
			//theplayer->OnDeath();
		}
	}
	else
	{
		if(mRotateValue < -2)
		{
			mChangeInRotation = 0.001;
		}
		else if (mRotateValue > 2)
		{
			mChangeInRotation = -0.001;
		}
		mRotateValue += mChangeInRotation;
		Body->setRotation(hkQuaternion(hkVector4(0,1,0),mRotateValue));
	}
}

NavigationPath* NavigationMesh::straightenPath( NavigationPath* path, Ogre::Radian maxTurnAngle, Ogre::Real pathWidth )
{
    NavigationPath* straightenedPath = new NavigationPath;
    NavigationPath::iterator startPoint;
    NavigationPath::iterator endPoint;
    Ogre::Vector3 rightAngleOffset;
    Ogre::Vector3 originalDirection;
    Ogre::Vector3 currentDirection;
    Ogre::Real slope;

    startPoint = path->begin();

    straightenedPath->push_back( *startPoint );

    while( startPoint != path->end() )
    {
        endPoint = startPoint;
        endPoint++;

        originalDirection = *endPoint - *startPoint;
        originalDirection.normalise();
        currentDirection = originalDirection;

        while( endPoint != path->end() )
        {
            slope = currentDirection.y;
            currentDirection = *endPoint - *startPoint;
            currentDirection.normalise();
            rightAngleOffset.x = currentDirection.z;
            rightAngleOffset.y = 0;
            rightAngleOffset.z = -currentDirection.x;
            rightAngleOffset *= pathWidth;

            // Test this line: startPoint -> endPoint

            // Do we turn to much in one spot?
            if( originalDirection.angleBetween( currentDirection ) > maxTurnAngle )
                break;

            // Does the path slope change?
            if( Ogre::Math::Abs(slope - currentDirection.y) > 0.01 )
                break;

            NavigationCell* prevCell;
            NavigationCell* testCell;
            NavigationCell* nextCell;

            Ogre::Vector3 sideStartPoint;
            Ogre::Vector3 sideEndPoint;
            NavigationCell::LINE_CLASSIFICATION ret;

            sideStartPoint = *startPoint + rightAngleOffset;
            sideEndPoint   = *endPoint   + rightAngleOffset;

            prevCell = getExactCellContainingPoint( sideStartPoint );
            if( prevCell )
            {
                ret = prevCell->classifyLine2D( sideStartPoint, sideEndPoint, prevCell, testCell );
                while( ret == NavigationCell::LINE_EXITS && testCell != 0 )
                {
                    ret = testCell->classifyLine2D( sideStartPoint, sideEndPoint, prevCell, nextCell );
                    prevCell = testCell;
                    testCell = nextCell;
                }

                if( testCell == 0 ) // Line leaves the navigation mesh.
                    break;
            }

            sideStartPoint = *startPoint - rightAngleOffset;
            sideEndPoint   = *endPoint   - rightAngleOffset;

            prevCell = getExactCellContainingPoint( sideStartPoint );
            if( prevCell )
            {
                ret = prevCell->classifyLine2D( sideStartPoint, sideEndPoint, prevCell, testCell );
                while( ret == NavigationCell::LINE_EXITS && testCell != 0 )
                {
                    ret = testCell->classifyLine2D( sideStartPoint, sideEndPoint, prevCell, nextCell );
                    prevCell = testCell;
                    testCell = nextCell;
                }

                if( testCell == 0 ) // Line leaves the navigation mesh.
                    break;
            }

            // We survived all the tests.  Move to next the point.
            endPoint++;
        }

        // Either we have got to the end of the points, or a point failed a test.

        if( startPoint >= (endPoint - 1))
        {
            // Point directly after startPoint failed a test... all we can do is just move along.
            startPoint++;
        }
        else
        {
//.........这里部分代码省略.........

tdt::real PhysicsHelper::get_angle(Ogre::Vector3 v1, Ogre::Vector3 v2)
{
	return v1.angleBetween(v2).valueRadians();
}