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

void Set::setSoundPosition(const char *soundName, const Math::Vector3d &pos, int minVol, int maxVol) {
	// TODO: The volume and pan needs to be updated when the setup changes.
	Math::Vector3d cameraPos = _currSetup->_pos;
	Math::Vector3d vector = pos - cameraPos;
	float distance = vector.getMagnitude();
	float diffVolume = maxVol - minVol;
	//This 8.f is a guess, so it may need some adjusting
	int newVolume = (int)(8.f * diffVolume / distance);
	newVolume += minVol;
	if (newVolume > _maxVolume)
		newVolume = _maxVolume;
	g_sound->setVolume(soundName, newVolume);

	Math::Vector3d cameraVector =_currSetup->_interest - _currSetup->_pos;
	Math::Vector3d up(0,0,1);
	Math::Vector3d right;
	cameraVector.normalize();
	float roll = -_currSetup->_roll * LOCAL_PI / 180.f;
	float cosr = cos(roll);
	// Rotate the up vector by roll.
	up = up * cosr + Math::Vector3d::crossProduct(cameraVector, up) * sin(roll) +
		cameraVector * Math::Vector3d::dotProduct(cameraVector, up) * (1 - cosr);
	right = Math::Vector3d::crossProduct(cameraVector, up);
	right.normalize();
	float angle = atan2(Math::Vector3d::dotProduct(vector, right),
						Math::Vector3d::dotProduct(vector, cameraVector));
	float pan = sin(angle);
	g_sound->setPan(soundName, (int)((pan + 1.f) / 2.f * 127.f + 0.5f));
}

void ShaderRenderer::screenPosToDirection(const Common::Point screen, float &pitch, float &heading) {
  double x, y, z;

  x = screen.x;
  y = kOriginalHeight - screen.y;
  z = 0.9f;

  const Math::Vector2d tl = _viewport.getTopLeft();
  x = 2 * double(x - tl.getX()) / _viewport.getWidth() - 1.0f;
  y = 2 * double(y - tl.getY()) / _viewport.getHeight() - 1.0f;
  z = 2 * z - 1.0f;

  // Screen coords to 3D coords
  Math::Vector4d point = Math::Vector4d(x, y, z, 1.0f);
  point = _mvpMatrix * point;

  // 3D coords to polar coords
  Math::Vector3d v = Math::Vector3d(point.x(), point.y(), point.z());
  v.normalize();

  Math::Vector2d horizontalProjection = Math::Vector2d(v.x(), v.z());
  horizontalProjection.normalize();

  pitch = 90 - Math::Angle::arcCosine(v.y()).getDegrees();
  heading = Math::Angle::arcCosine(horizontalProjection.getY()).getDegrees();

  if (horizontalProjection.getX() > 0.0)
    heading = 360 - heading;
}

void Actor::updateWalk() {
	if (_path.empty()) {
		return;
	}

	Math::Vector3d destPos = _path.back();
	Math::Vector3d dir = destPos - _pos;
	float dist = dir.getMagnitude();

	_walkedCur = true;
	float walkAmt = g_grim->getPerSecond(_walkRate);
	if (walkAmt >= dist) {
		_path.pop_back();
		if (_path.empty()) {
			_walking = false;
			_pos = destPos;
// It seems that we need to allow an already active turning motion to
// continue or else turning actors away from barriers won't work right
			_turning = false;
			return;
		}
	}

	destPos = handleCollisionTo(_pos, destPos);
	Math::Angle y = getYawTo(destPos);
	turnTo(_pitch, y, _roll);

	dir = destPos - _pos;
	dir.normalize();
	_pos += dir * walkAmt;
}

static void polarRectTo3dRect(const PolarRect &polarRect,
                              Math::Vector3d &topLeft, Math::Vector3d &topRight,
                              Math::Vector3d &bottomLeft, Math::Vector3d &bottomRight) {
	static const float scale = 50.0;

	Math::Vector3d direction = Scene::directionToVector(polarRect.centerPitch, 90.0 - polarRect.centerHeading) * scale;

	Math::Vector3d u = Math::Vector3d(direction.z(), 0.0, -direction.x());
	u.normalize();

	Math::Vector3d v = Math::Vector3d::crossProduct(direction, u);
	v.normalize();

	Math::Vector3d sizeU = u * polarRect.width  / 90.0 * scale;
	Math::Vector3d sizeV = v * polarRect.height / 90.0 * scale;

	topRight = direction + sizeV + sizeU;
	bottomRight = direction - sizeV + sizeU;
	bottomLeft = direction - sizeV - sizeU;
	topLeft = direction + sizeV - sizeU;
}

/** 
 * Generates a lookat matrix. For reference, see 
 * http://clb.demon.fi/MathGeoLib/docs/float3x3_LookAt.php 
 */
void Matrix<3, 3>::buildFromTargetDir(const Math::Vector3d &modelForward, const Math::Vector3d &targetDirection, 
						   const Math::Vector3d &modelUp, const Math::Vector3d &worldUp)
{
    Math::Vector3d modelRight = Math::Vector3d::crossProduct(modelUp, modelForward);
	modelRight.normalize();
	Math::Vector3d worldRight = Math::Vector3d::crossProduct(worldUp, targetDirection);
	worldRight.normalize();
	Math::Vector3d perpWorldUp = Math::Vector3d::crossProduct(targetDirection, worldRight);
	perpWorldUp.normalize();
	
	Math::Matrix3 m1;
	m1.getRow(0) << worldRight.x() << worldRight.y() << worldRight.z();
	m1.getRow(1) << perpWorldUp.x() << perpWorldUp.y() << perpWorldUp.z();
	m1.getRow(2) << targetDirection.x() << targetDirection.y() << targetDirection.z();
	m1.transpose();
	
	Math::Matrix3 m2;
	m2.getRow(0) << modelRight.x() << modelRight.y() << modelRight.z();
	m2.getRow(1) << modelUp.x() << modelUp.y() << modelUp.z();
	m2.getRow(2) << modelForward.x() << modelForward.y() << modelForward.z();
	
	this->operator=(m1 * m2);
}

void BaseRenderer::screenPosToDirection(const Common::Point screen, float &pitch, float &heading) {
	// Screen coords to 3D coords
	Math::Vector3d obj;
	Math::gluMathUnProject(Math::Vector3d(screen.x, _system->getHeight() - screen.y, 0.9f), _mvpMatrix, frameViewport(), obj);

	// 3D coords to polar coords
	obj.normalize();

	Math::Vector2d horizontalProjection = Math::Vector2d(obj.x(), obj.z());
	horizontalProjection.normalize();

	pitch = 90 - Math::Angle::arcCosine(obj.y()).getDegrees();
	heading = Math::Angle::arcCosine(horizontalProjection.getY()).getDegrees();

	if (horizontalProjection.getX() > 0.0)
		heading = 360 - heading;
}

void TinyGLRenderer::screenPosToDirection(const Common::Point screen, float &pitch, float &heading) {
	// Screen coords to 3D coords
	Math::Vector3d obj;
	Math::gluMathUnProject<float, int>(Math::Vector3d(screen.x, kOriginalHeight - screen.y, 0.9),
		_cubeModelViewMatrix, _cubeProjectionMatrix, _cubeViewport, obj);

	// 3D coords to polar coords
	obj.normalize();

	Math::Vector2d horizontalProjection = Math::Vector2d(obj.x(), obj.z());
	horizontalProjection.normalize();

	pitch = 90 - Math::Angle::arcCosine(obj.y()).getDegrees();
	heading = Math::Angle::arcCosine(horizontalProjection.getY()).getDegrees();

	if (horizontalProjection.getX() > 0.0)
		heading = 360 - heading;
}

void Renderer::screenPosToDirection(const Common::Point screen, float &pitch, float &heading) {
	double x, y, z;

	// Screen coords to 3D coords
	gluUnProject(screen.x, kOriginalHeight - screen.y, 0.9, _cubeModelViewMatrix, _cubeProjectionMatrix, (GLint *)_cubeViewport, &x, &y, &z);

	// 3D coords to polar coords
	Math::Vector3d v = Math::Vector3d(x, y, z);
	v.normalize();

	Math::Vector2d horizontalProjection = Math::Vector2d(v.x(), v.z());
	horizontalProjection.normalize();

	pitch = 90 - Math::Angle::arcCosine(v.y()).getDegrees();
	heading = Math::Angle::arcCosine(horizontalProjection.getY()).getDegrees();

	if (horizontalProjection.getX() > 0.0)
		heading = 360 - heading;
}

void EMIHead::lookAt(bool entering, const Math::Vector3d &point, float rate, const Math::Matrix4 &matrix) {
	if (!_cost->_emiSkel || !_cost->_emiSkel->_obj)
		return;

	if (_jointName.empty())
		return;

	Joint *joint = _cost->_emiSkel->_obj->getJointNamed(_jointName);
	if (!joint)
		return;

	Math::Quaternion lookAtQuat; // Note: Identity if not looking at anything.

	if (entering) {
		Math::Matrix4 jointToWorld = _cost->getOwner()->getFinalMatrix() * joint->_finalMatrix;
		Math::Vector3d jointWorldPos = jointToWorld.getPosition();
		Math::Matrix4 worldToJoint = jointToWorld;
		worldToJoint.invertAffineOrthonormal();

		Math::Vector3d targetDir = (point + _offset) - jointWorldPos;
		targetDir.normalize();

		const Math::Vector3d worldUp(0, 1, 0);
		Math::Vector3d frontDir = Math::Vector3d(worldToJoint(0, 1), worldToJoint(1, 1), worldToJoint(2, 1)); // Look straight ahead. (+Y)
		Math::Vector3d modelFront(0, 0, 1);
		Math::Vector3d modelUp(0, 1, 0);

		joint->_absMatrix.inverseRotate(&modelFront);
		joint->_absMatrix.inverseRotate(&modelUp);

		// Generate a world-space look at matrix.
		Math::Matrix4 lookAtTM;
		lookAtTM.setToIdentity();

		if (Math::Vector3d::dotProduct(targetDir, worldUp) >= 0.98f) // Avoid singularity if trying to look straight up.
			lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, -frontDir); // Instead of orienting head towards scene up, orient head towards character "back",
		else if (Math::Vector3d::dotProduct(targetDir, worldUp) <= -0.98f) // Avoid singularity if trying to look straight down.
			lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, frontDir); // Instead of orienting head towards scene down, orient head towards character "front",
		else
			lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, worldUp);

		// Convert from world-space to joint-space.
		lookAtTM = worldToJoint * lookAtTM;

		// Apply angle limits.
		Math::Angle p, y, r;
		lookAtTM.getXYZ(&y, &p, &r, Math::EO_ZXY);

		y.clampDegrees(_yawRange);
		p.clampDegrees(_minPitch, _maxPitch);
		r.clampDegrees(30.0f);

		lookAtTM.buildFromXYZ(y, p, r, Math::EO_ZXY);

		lookAtQuat.fromMatrix(lookAtTM.getRotation());
	}

	if (_headRot != lookAtQuat) {
		Math::Quaternion diff = _headRot.inverse() * lookAtQuat;
		float angle = 2 * acos(diff.w());
		if (diff.w() < 0.0f) {
			angle = 2 * (float)M_PI - angle;
		}

		float turnAmount = g_grim->getPerSecond(rate * ((float)M_PI / 180.0f));
		if (turnAmount < angle)
			_headRot = _headRot.slerpQuat(lookAtQuat, turnAmount / angle);
		else
			_headRot = lookAtQuat;
	}

	if (_headRot != Math::Quaternion()) { // If not identity..
		joint->_animMatrix = joint->_animMatrix * _headRot.toMatrix();
		joint->_animQuat = joint->_animQuat * _headRot;
		_cost->_emiSkel->_obj->commitAnim();
	}
}

void Sector::shrink(float radius) {
	if ((getType() & WalkType) == 0 || _shrinkRadius == radius)
		return;

	_shrinkRadius = radius;
	if (!_origVertices) {
		_origVertices = _vertices;
		_vertices = new Math::Vector3d[_numVertices + 1];
	}

	// Move each vertex inwards by the given amount.
	for (int j = 0; j < _numVertices; j++) {
		Math::Vector3d shrinkDir;

		for (int k = 0; k < g_grim->getCurrSet()->getSectorCount(); k++) {
			Sector *other = g_grim->getCurrSet()->getSectorBase(k);
			if ((other->getType() & WalkType) == 0)
				continue;

			for (int l = 0; l < other->_numVertices; l++) {
				Math::Vector3d *otherVerts = other->_vertices;
				if (other->_origVertices)
					otherVerts = other->_origVertices;
				if ((otherVerts[l] - _origVertices[j]).getMagnitude() < 0.01f) {
					Math::Vector3d e1 = otherVerts[l + 1] - otherVerts[l];
					Math::Vector3d e2;
					if (l - 1 >= 0)
						e2 = otherVerts[l] - otherVerts[l - 1];
					else
						e2 = otherVerts[l] - otherVerts[other->_numVertices - 1];
					e1.normalize();
					e2.normalize();
					Math::Vector3d bisector = (e1 - e2);
					bisector.normalize();
					shrinkDir += bisector;
				}
			}
		}

		if (shrinkDir.getMagnitude() > 0.1f) {
			shrinkDir.normalize();
			_vertices[j] = _origVertices[j] + shrinkDir * radius;
		} else {
			_vertices[j] = _origVertices[j];
		}
	}

	_vertices[_numVertices] = _vertices[0];

	// Make sure the sector is still convex.
	for (int j = 0; j < _numVertices; j++) {
		Math::Vector3d e1 = _vertices[j + 1] - _vertices[j];
		Math::Vector3d e2;
		if (j - 1 >= 0)
			e2 = _vertices[j] - _vertices[j - 1];
		else
			e2 = _vertices[j] - _vertices[_numVertices - 1];

		if (e1.x() * e2.y() > e1.y() * e2.x()) {
			// Not convex, so mark the sector invalid.
			_invalid = true;
			delete[] _vertices;
			_vertices = _origVertices;
			_origVertices = nullptr;
			break;
		}
	}
}

Math::Vector3d Path3D::getEdgeDirection(uint edgeIndex) const {
	Math::Vector3d direction = getVertexPosition(edgeIndex) - getVertexPosition(edgeIndex + 1);
	direction.normalize();
	return direction;
}

void Head::Joint::orientTowards(bool entering, const Math::Vector3d &point, float rate, const Math::Matrix4 &matrix,
	float maxPitch, float maxYaw, float maxRoll, float constrain) {
	float step = g_grim->getPerSecond(rate);
	float yawStep = step;
	float pitchStep = step / 3.0f;
	float rollStep = step / 3.0f;

	if (!_node)
		return;

	// Make sure we have up-to-date world transform matrices computed for the joint nodes of this character.
	_node->_needsUpdate = true;
	ModelNode *p = _node;
	while (p->_parent) {
		p = p->_parent;
		p->_needsUpdate = true;
	}
	p->setMatrix(matrix);
	p->update();

	Math::Vector3d modelFront; // the modeling convention for the forward direction.
	Math::Vector3d modelUp; // the modeling convention for the upward direction.
	Math::Vector3d frontDir; // Character front facing direction vector in world space (global scene coordinate space)

	// the character head coordinate frame is: +Y forward, +Z up, +X right.
	frontDir = Math::Vector3d(_node->_matrix(0,1), _node->_matrix(1,1), _node->_matrix(2,1)); // Look straight ahead. (+Y)
	modelFront = Math::Vector3d(0,1,0);
	modelUp = Math::Vector3d(0,0,1);

	// v is the world space direction vector this character should be looking towards.
	Math::Vector3d targetDir = point - _node->_pivotMatrix.getPosition();
	if (!entering)
		targetDir = frontDir;
	if (targetDir.isZero())
		return;

	targetDir.normalize();

	// The vector v is in world space, so generate the world space lookat matrix for the desired head facing
	// orientation.
	Math::Matrix4 lookAtTM;
	lookAtTM.setToIdentity();
	const Math::Vector3d worldUp(0,0,1); // The Residual scene convention: +Z is world space up.
	if (Math::Vector3d::dotProduct(targetDir, worldUp) >= 0.98f) // Avoid singularity if trying to look straight up.
		lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, -frontDir); // Instead of orienting head towards scene up, orient head towards character "back",
	                                                                // i.e. when you look straight up, your head up vector tilts/arches to point straight backwards.
	else if (Math::Vector3d::dotProduct(targetDir, worldUp) <= -0.98f) // Avoid singularity if trying to look straight down.
		lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, frontDir); // Instead of orienting head towards scene down, orient head towards character "front",
																   // i.e. when you look straight down, your head up vector tilts/arches to point straight forwards.
	else
		lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, worldUp);
	// The above specifies the world space orientation of this bone, but we need to output
	// the orientation in parent space (as yaw/pitch/roll).

	// Get the coordinate frame in which we need to produce the character head yaw/pitch/roll values.
	Math::Matrix4 parentWorldTM;
	if (_node->_parent)
		parentWorldTM = _node->_parent->_matrix;

	// While we could compute the desired lookat direction directly in the above coordinate frame,
	// it is preferrable to compute the lookat direction with respect to the head orientation in
	// the keyframe animation. This is because the LUA scripts specify the maximum head yaw, pitch and
	// roll values with respect to those keyframe animations. If the lookat was simply computed
	// directly in the space of the parent, we couldn't apply the head maxYaw/Pitch/Roll constraints
	// properly. So, compute the coordinate frame of this bone in the keyframe animation.
	Math::Matrix4 animFrame = _node->_localMatrix;
	parentWorldTM = parentWorldTM * animFrame;
	parentWorldTM.invertAffineOrthonormal();

	// Convert lookAtTM orientation from world space to parent-with-keyframe-animation space.
	lookAtTM = parentWorldTM * lookAtTM;

	// Decompose to yaw-pitch-roll (+Z, +X, +Y).
	// In this space, Yaw is +Z. Pitch is +X. Roll is +Y.
	Math::Angle y, pt, r;
	lookAtTM.getPitchYawRoll(&pt, &y, &r);

	y = y * constrain;
	pt = pt * constrain;
	r = r * constrain;

	// Constrain the maximum head movement, as desired by the game LUA scripts.
	y.clampDegrees(maxYaw);
	pt.clampDegrees(maxPitch);
	r.clampDegrees(maxRoll);

	// Also limit yaw, pitch and roll to make at most a movement as large as the given max step size during this frame.
	// This will produce a slow head-turning animation instead of immediately snapping to the
	// target lookat orientation.
	if (y - _yaw > yawStep)
		y = _yaw + yawStep;
	if (_yaw - y > yawStep)
		y = _yaw - yawStep;

	if (pt - _pitch > pitchStep)
		pt = _pitch + pitchStep;
	if (_pitch - pt > pitchStep)
		pt = _pitch - pitchStep;

	if (r - _roll > rollStep)
//.........这里部分代码省略.........

void Walk::onGameLoop() {
	if (!_path->hasSteps()) {
		// There is no path to the destination
		stop();
		return;
	}

	Resources::Floor *floor = StarkGlobal->getCurrent()->getFloor();

	// Get the target to walk to
	Math::Vector3d currentPosition = _item3D->getPosition3D();
	Math::Vector3d target = _path->computeWalkTarget(currentPosition);

	// Compute the direction to walk into
	Math::Vector3d direction = target - currentPosition;
	direction.z() = 0;
	direction.normalize();

	// Compute the angle with the current character direction
	Math::Vector3d currentDirection = _item3D->getDirectionVector();
	float directionDeltaAngle = computeAngleBetweenVectorsXYPlane(currentDirection, direction);

	// If the angle between the current direction and the new one is too high,
	// make the character turn on itself until the angle is low enough
	if (ABS(directionDeltaAngle) > getAngularSpeed() + 0.1f) {
		_turnDirection = directionDeltaAngle < 0 ? kTurnLeft : kTurnRight;
	} else {
		_turnDirection = kTurnNone;
	}

	float distancePerGameloop = computeDistancePerGameLoop();

	Math::Vector3d newPosition;
	if (_turnDirection == kTurnNone) {
		// Compute the new position using the distance per gameloop
		if (currentPosition.getDistanceTo(target) > distancePerGameloop) {
			newPosition = currentPosition + direction * distancePerGameloop;
		} else {
			newPosition = target;
		}
	} else {
		// The character does not change position when it is turning
		newPosition = currentPosition;
		direction = currentDirection;

		Math::Matrix3 rot;
		rot.buildAroundZ(_turnDirection == kTurnLeft ? -getAngularSpeed() : getAngularSpeed());
		rot.transformVector(&direction);
	}

	// Some scripts expect the character position to be the exact destination
	if (newPosition == _destination) {
		_reachedDestination = true;
		stop();
	}

	// Update the new position's height according to the floor
	int32 newFloorFaceIndex = floor->findFaceContainingPoint(newPosition);
	if (newFloorFaceIndex >= 0) {
		floor->computePointHeightInFace(newPosition, newFloorFaceIndex);
	} else {
		warning("Item %s is walking off the floor", _item->getName().c_str());
	}

	// Update the item's properties
	_item3D->setPosition3D(newPosition);
	if (direction.getMagnitude() != 0.0) {
		_item3D->setDirection(computeAngleBetweenVectorsXYPlane(direction, Math::Vector3d(1.0, 0.0, 0.0)));
	}
	if (newFloorFaceIndex >= 0) {
		// When unable to find the face containing the new position, keep the previous one
		// to prevent draw order glitches.
		_item3D->setFloorFaceIndex(newFloorFaceIndex);
	}

	changeItemAnim();
}

bool Actor::handleCollisionWith(Actor *actor, CollisionMode mode, Math::Vector3d *vec) const {
	if (actor->_collisionMode == CollisionOff) {
		return false;
	}

	Model *model1 = getCurrentCostume()->getModel();
	Model *model2 = actor->getCurrentCostume()->getModel();

	Math::Vector3d p1 = _pos + model1->_insertOffset;
	Math::Vector3d p2 = actor->_pos + model2->_insertOffset;

	float size1 = model1->_radius * _collisionScale;
	float size2 = model2->_radius * actor->_collisionScale;

	CollisionMode mode1 = mode;
	CollisionMode mode2 = actor->_collisionMode;

	if (mode1 == CollisionSphere && mode2 == CollisionSphere) {
		Math::Vector3d pos = p1 + *vec;
		float distance = (pos - p2).getMagnitude();
		if (distance < size1 + size2) {
			// Move the destination point so that its distance from the
			// center of the circle is size1+size2.
			Math::Vector3d v = pos - p2;
			v.normalize();
			v *= size1 + size2;
			*vec = v + p2 - p1;

			collisionHandlerCallback(actor);
			return true;
		}
	} else if (mode1 == CollisionBox && mode2 == CollisionBox) {
		warning("Collision between box and box not implemented!");
		return false;
	} else {
		Math::Rect2d rect;

		Math::Vector3d bboxPos;
		Math::Vector3d size;
		float scale;
		Math::Vector3d pos;
		Math::Vector3d circlePos;
		Math::Angle yaw;

		Math::Vector2d circle;
		float radius;

		if (mode1 == CollisionBox) {
			pos = p1 + *vec;
			bboxPos = pos + model1->_bboxPos;
			size = model1->_bboxSize;
			scale = _collisionScale;
			yaw = _yaw;

			circle.setX(p2.x());
			circle.setY(p2.y());
			circlePos = p2;
			radius = size2;
		} else {
			pos = p2;
			bboxPos = p2 + model2->_bboxPos;
			size = model2->_bboxSize;
			scale = actor->_collisionScale;
			yaw = actor->_yaw;

			circle.setX(p1.x() + vec->x());
			circle.setY(p1.y() + vec->y());
			circlePos = p1;
			radius = size1;
		}

		rect._topLeft = Math::Vector2d(bboxPos.x(), bboxPos.y() + size.y());
		rect._topRight = Math::Vector2d(bboxPos.x() + size.x(), bboxPos.y() + size.y());
		rect._bottomLeft = Math::Vector2d(bboxPos.x(), bboxPos.y());
		rect._bottomRight = Math::Vector2d(bboxPos.x() + size.x(), bboxPos.y());

		rect.scale(scale);
		rect.rotateAround(Math::Vector2d(pos.x(), pos.y()), yaw);

		if (rect.intersectsCircle(circle, radius)) {
			Math::Vector2d center = rect.getCenter();
			// Draw a line from the center of the rect to the place the character
			// would go to.
			Math::Vector2d v = circle - center;
			v.normalize();

			Math::Segment2d edge;
			// That line intersects (usually) an edge
			rect.getIntersection(center, v, &edge);
			// Take the perpendicular of that edge
			Math::Line2d perpendicular = edge.getPerpendicular(circle);

			Math::Vector3d point;
			Math::Vector2d p;
			// If that perpendicular intersects the edge
			if (edge.intersectsLine(perpendicular, &p)) {
				Math::Vector2d direction = perpendicular.getDirection();
				direction.normalize();

				// Move from the intersection until we are at a safe distance
//.........这里部分代码省略.........