C++ Vector::normalize方法代码示例

/// \brief Get an accurate height and normal vector at a given coordinate
/// relative to this segment.
///
/// The height and surface normal are determined by finding the four adjacent
/// height points nearest to the coordinate, and interpolating between
/// those height values. The square area defined by the 4 height points is
/// considered as two triangles for the purposes of interpolation to ensure
/// that the calculated height falls on the surface rendered by a 3D
/// graphics engine from the same heightfield data. The line used to
/// divide the area is defined by the gradient y = x, so the first
/// triangle has relative vertex coordinates (0,0) (1,0) (1,1) and
/// the second triangle has vertex coordinates (0,0) (0,1) (1,1).
void Segment::getHeightAndNormal(float x, float y, float& h,
                                 WFMath::Vector<3> &normal) const
{
    // FIXME this ignores edges and corners
    assert(x <= m_res);
    assert(x >= 0.0f);
    assert(y <= m_res);
    assert(y >= 0.0f);
    
    // get index of the actual tile in the segment
    int tile_x = I_ROUND(std::floor(x));
    int tile_y = I_ROUND(std::floor(y));

    // work out the offset into that tile
    float off_x = x - tile_x;
    float off_y = y - tile_y;
 
    float h1=get(tile_x, tile_y);
    float h2=get(tile_x, tile_y+1);
    float h3=get(tile_x+1, tile_y+1);
    float h4=get(tile_x+1, tile_y);

    // square is broken into two triangles
    // top triangle |/
    if ((off_x - off_y) <= 0.f) {
        normal = WFMath::Vector<3>(h2-h3, h1-h2, 1.0f);

        //normal for intersection of both triangles
        if (off_x == off_y) {
            normal += WFMath::Vector<3>(h1-h4, h4-h3, 1.0f);
        }
        normal.normalize();
        h = h1 + (h3-h2) * off_x + (h2-h1) * off_y;
    } 
    // bottom triangle /|
    else {
        normal = WFMath::Vector<3>(h1-h4, h4-h3, 1.0f);
        normal.normalize();
        h = h1 + (h4-h1) * off_x + (h3-h4) * off_y;
    }
}

void TerrainPageShadow::updateShadow(const TerrainPageGeometry& geometry)
{
	if (!mImage) {
		mImage = new OgreImage(new Image::ImageBuffer(mTerrainPage.getBlendMapSize(), 1));
	}
	mImage->reset();


	int pageSizeInVertices = mTerrainPage.getPageSize();
	int pageSizeInMeters = pageSizeInVertices - 1;

	//since Ogre uses a different coord system than WF, we have to do some conversions here
	TerrainPosition origPosition(0, pageSizeInMeters - 1);

	WFMath::Vector<3> wfLightDirection = mLightDirection;
	wfLightDirection = wfLightDirection.normalize(1);

	TerrainPosition position(origPosition);

	auto data = mImage->getData();

	for (int i = 0; i < pageSizeInMeters; ++i) {
		position = origPosition;
		position[1] = position[1] - i;
		for (int j = 0; j < pageSizeInMeters; ++j) {
			WFMath::Vector<3> normal;
			if (geometry.getNormal(position, normal)) {
				float dotProduct = WFMath::Dot(normal.normalize(1), wfLightDirection);

				// if the dotProduct is > 0, the face is looking away from the sun
				*data = static_cast<unsigned char>((1.0f - ((dotProduct + 1.0f) * 0.5f)) * 255);

			} else {
				*data = 0;
			}
			data++;
			position[0] = position[0] + 1;
		}
	}

}

void Steering::update()
{
	if (mSteeringEnabled) {
		if (mUpdateNeeded) {
			updatePath();
		}
		auto entity = mAvatar.getEntity();
		if (!mPath.empty()) {
			const auto& finalDestination = mPath.back();
			auto entity3dPosition = entity->getViewPosition();
			const WFMath::Point<2> entityPosition(entity3dPosition.x(), entity3dPosition.z());
			//First check if we've arrived at our actual destination.
			if (WFMath::Distance(WFMath::Point<2>(finalDestination.x(), finalDestination.z()), entityPosition) < 0.1f) {
				//We've arrived at our destination. If we're moving we should stop.
				if (mLastSentVelocity.isValid() && mLastSentVelocity != WFMath::Vector<2>::ZERO()) {
					moveInDirection(WFMath::Vector<2>::ZERO());
				}
				stopSteering();
			} else {
				//We should send a move op if we're either not moving, or we've reached a waypoint, or we need to divert a lot.

				WFMath::Point<2> nextWaypoint(mPath.front().x(), mPath.front().z());
				if (WFMath::Distance(nextWaypoint, entityPosition) < 0.1f) {
					mPath.pop_front();
					nextWaypoint = WFMath::Point<2>(mPath.front().x(), mPath.front().z());
				}

				WFMath::Vector<2> velocity = nextWaypoint - entityPosition;
				WFMath::Point<2> destination;
				velocity.normalize();

				if (mPath.size() == 1) {
					//if the next waypoint is the destination we should send a "move to position" update to the server, to make sure that we stop when we've arrived.
					//otherwise, if there's too much lag, we might end up overshooting our destination and will have to double back
					destination = nextWaypoint;
				}

				//Check if we need to divert in order to avoid colliding.
				WFMath::Vector<2> newVelocity;
				bool avoiding = mAwareness.avoidObstacles(entityPosition, velocity * mSpeed, newVelocity);
				if (avoiding) {
					auto newMag = newVelocity.mag();
					auto relativeMag = mSpeed / newMag;

					velocity = newVelocity;
					velocity.normalize();
					velocity *= relativeMag;
					mUpdateNeeded = true;
				}

				bool shouldSend = false;
				if (velocity.isValid()) {
					if (mLastSentVelocity.isValid()) {
						//If the entity has stopped, and we're not waiting for confirmation to a movement request we've made, we need to start moving.
						if (!entity->isMoving() && !mExpectingServerMovement) {
							shouldSend = true;
						} else {
							auto currentTheta = std::atan2(mLastSentVelocity.y(), mLastSentVelocity.x());
							auto newTheta = std::atan2(velocity.y(), velocity.x());

							//If we divert too much from where we need to go we must adjust.
							if (std::abs(currentTheta - newTheta) > WFMath::numeric_constants<double>::pi() / 20) {
								shouldSend = true;
							}
						}
					} else {
						//If we've never sent a movement update before we should do that now.
						shouldSend = true;
					}
				}
				if (shouldSend) {
					//If we're moving to a certain destination and aren't avoiding anything we should tell the server to move to the destination.
					if (destination.isValid() && !avoiding) {
						moveToPoint(WFMath::Point<3>(destination.x(), entity3dPosition.y(), destination.y()));
					} else {
						moveInDirection(velocity);
					}
				}
			}
		} else {
			//We are steering, but the path is empty, which means we can't find any path. If we're moving we should stop movement.
			//But we won't stop steering; perhaps we'll find a path later.
			if (mLastSentVelocity.isValid() && mLastSentVelocity != WFMath::Vector<2>::ZERO()) {
				moveInDirection(WFMath::Vector<2>::ZERO());
			}
		}
	}

}

//.........这里部分代码省略.........
    }


    WFMath::Point<3> intPoint;
    WFMath::Vector<3> intNorm;
    float par;
    //test vertical ray
    if (Mercator::Intersect(terrain, WFMath::Point<3>(20.1, 20.2, segmax + 3), 
                               WFMath::Vector<3>(0.0,0.0,50.0), intPoint, intNorm, par)) {
        std::cerr << "vertical ray intersected when it shouldnt" << std::endl;
        return 1;
    }
    
    if (!Mercator::Intersect(terrain, WFMath::Point<3>(20.1, 20.2, segmax + 3), 
                               WFMath::Vector<3>(0.0,0.0,-50.0), intPoint, intNorm, par)) {
        std::cerr << "vertical ray didnt intersect when it should" << std::endl;
        return 1;
    }

    //test each quadrant
    if (!Mercator::Intersect(terrain, WFMath::Point<3>(20.1, 20.2, segmax + 3), 
                               WFMath::Vector<3>(10.0,10.0,-100.0), intPoint, intNorm, par)) {
        std::cerr << "quad1 ray didnt intersect when it should" << std::endl;
        return 1;
    }

    if (!Mercator::Intersect(terrain, WFMath::Point<3>(20.1, 20.2, segmax + 3), 
                               WFMath::Vector<3>(10.0,-15.0,-50.0), intPoint, intNorm, par)) {
        std::cerr << "quad2 ray didnt intersect when it should" << std::endl;
        return 1;
    }

    if (!Mercator::Intersect(terrain, WFMath::Point<3>(20.1, 20.2, segmax + 3), 
                               WFMath::Vector<3>(-10.0,-10.0,-50.0), intPoint, intNorm, par)) {
        std::cerr << "quad3 ray didnt intersect when it should" << std::endl;
        return 1;
    }

    if (!Mercator::Intersect(terrain, WFMath::Point<3>(20.1, 20.2, segmax + 3), 
                               WFMath::Vector<3>(-10.0,10.0,-50.0), intPoint, intNorm, par)) {
        std::cerr << "quad4 ray didnt intersect when it should" << std::endl;
        return 1;
    }
    
    //test dx==0 and dy==0
    if (!Mercator::Intersect(terrain, WFMath::Point<3>(20.1, 20.2, segmax + 3), 
                               WFMath::Vector<3>(0.0,10.0,-50.0), intPoint, intNorm, par)) {
        std::cerr << "y+ ray didnt intersect when it should" << std::endl;
        return 1;
    }

    if (!Mercator::Intersect(terrain, WFMath::Point<3>(20.1, 20.2, segmax + 3), 
                               WFMath::Vector<3>(0.0,-10.0,-50.0), intPoint, intNorm, par)) {
        std::cerr << "y- ray didnt intersect when it should" << std::endl;
        return 1;
    }

    if (!Mercator::Intersect(terrain, WFMath::Point<3>(20.1, 20.2, segmax + 3), 
                               WFMath::Vector<3>(-10.0,0.0,-50.0), intPoint, intNorm, par)) {
        std::cerr << "x- ray didnt intersect when it should" << std::endl;
        return 1;
    }

    if (!Mercator::Intersect(terrain, WFMath::Point<3>(20.1, 20.2, segmax + 3), 
                               WFMath::Vector<3>(10.0,0.0,-50.0), intPoint, intNorm, par)) {
        std::cerr << "x+ ray didnt intersect when it should" << std::endl;
        return 1;
    }
    
    //test a longer ray
    if (!Mercator::Intersect(terrain, WFMath::Point<3>(-10.08, -20.37, segmax + 3), 
                               WFMath::Vector<3>(100.0,183.0,-50.0), intPoint, intNorm, par)) {
        std::cerr << "long ray didnt intersect when it should" << std::endl;
        return 1;
    }
 
    //check the height value
    float h;
    WFMath::Vector<3> n;
    terrain.getHeightAndNormal(intPoint[0], intPoint[1], h, n);
    n.normalize();

    if (n != intNorm) {
        std::cerr << "calculated normal is different from getHeightAndNormal" << std::endl;
        std::cerr << intPoint << std::endl;
        std::cerr << intNorm << "!=" << n << std::endl;
       // return 1;
    }
    
    // We can't check for equality here is it just doesn't work with
    // floats. Look it up in any programming book if you don't believe me.
    //  - 20040721 <[email protected]>
    if (fabs(h - intPoint[2]) > 0.00001) {
        std::cerr << "calculated height is different from getHeightAndNormal" << std::endl;
        std::cerr << h << "!=" << intPoint[2] << std::endl;
        return 1;
    }
        
    return 0;
}