C++ math::Vector3d类代码示例

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;
}

void Lua_V2::WorldToScreen() {
	lua_Object xObj = lua_getparam(1);
	lua_Object yObj = lua_getparam(2);
	lua_Object zObj = lua_getparam(3);
	if (!lua_isnumber(xObj) || !lua_isnumber(yObj) || !lua_isnumber(zObj)) {
		lua_pushnumber(0.0);
		lua_pushnumber(0.0);
		return;
	}

	float x = lua_getnumber(xObj);
	float y = lua_getnumber(yObj);
	float z = lua_getnumber(zObj);
	Math::Vector3d pos = Math::Vector3d(x, y, z);

	const Set::Setup *setup = g_emi->getCurrSet()->getCurrSetup();
	const Math::Vector3d interest = setup->_interest;
	const float roll = setup->_roll;
	const Math::Quaternion quat = Math::Quaternion(interest.x(), interest.y(), interest.z(), roll);
	Math::Matrix4 view = quat.toMatrix();
	view.transpose();

	pos -= setup->_pos;
	pos = view.getRotation() * pos;
	pos.z() = -pos.z();

	Math::Matrix4 proj = GfxBase::makeProjMatrix(setup->_fov, setup->_nclip, setup->_fclip);
	proj.transpose();
	Math::Vector4d screen = proj * Math::Vector4d(pos.x(), pos.y(), pos.z(), 1.0);
	screen /= screen.w();

	lua_pushnumber((screen.x() + 1) * 320);
	lua_pushnumber((1 - screen.y()) * 240);
}

void Lua_V1::SetSoundPosition() {
	Math::Vector3d pos;
	int minVolume = 10;
	int maxVolume = 127;
	float someParam = 0;
	int argId = 1;
	lua_Object paramObj;

	if (g_grim->getCurrSet()) {
		g_grim->getCurrSet()->getSoundParameters(&minVolume, &maxVolume);
	}

	lua_Object nameObj = lua_getparam(argId++);
	if (!lua_isnumber(nameObj) && !lua_isstring(nameObj))
		return;

	lua_Object actorObj = lua_getparam(argId++);
	if (lua_isuserdata(actorObj) && lua_tag(actorObj) == MKTAG('A','C','T','R')) {
		Actor *actor = getactor(actorObj);
		if (!actor)
			return;
		pos = actor->getPos();
	} else if (lua_isnumber(actorObj)) {
		float x = lua_getnumber(actorObj);
		float y = lua_getnumber(argId++);
		float z = lua_getnumber(argId++);
		pos.set(x, y, z);
	}

	paramObj = (int)lua_getparam(argId++);
	if (lua_isnumber(paramObj)) {
		minVolume = (int)lua_getnumber(paramObj);
		if (minVolume > 127)
			minVolume = 127;
	}
	paramObj = lua_getparam(argId++);
	if (lua_isnumber(paramObj)) {
		maxVolume = (int)lua_getnumber(paramObj);
		if (maxVolume > 127)
			maxVolume = 127;
		else if (maxVolume < minVolume)
			maxVolume = minVolume;
	}

	paramObj = lua_getparam(argId++);
	if (lua_isnumber(paramObj)) {
		someParam = (int)lua_getnumber(paramObj);
		if (someParam < 0.0)
			someParam = 0.0;
	}

	if (g_grim->getCurrSet()) {
		if (lua_isnumber(nameObj))
			error("SetSoundPosition: number is not yet supported");
		else {
			const char *soundName = lua_getstring(nameObj);
			g_grim->getCurrSet()->setSoundPosition(soundName, pos, minVolume, maxVolume);
		}
	}
}

void SoundTrack::updatePosition() {
    if (!_positioned)
        return;

    Set *set = g_grim->getCurrSet();
    Set::Setup *setup = set->getCurrSetup();
    Math::Vector3d cameraPos = setup->_pos;
    Math::Vector3d vector = _pos - cameraPos;
    float distance = vector.getMagnitude();
    _attenuation = MAX(0.0f, 1.0f - distance / (_volume * 100.0f / Audio::Mixer::kMaxChannelVolume));
    if (!isfinite(_attenuation)) {
        _attenuation = 0.0f;
    }

    Math::Matrix4 worldRot = setup->_rot;
    Math::Vector3d relPos = (_pos - setup->_pos);
    Math::Vector3d p(relPos);
    worldRot.inverseRotate(&p);
    float angle = atan2(p.x(), p.z());
    float pan = sin(angle);
    _balance = (int)(pan * 127.0f);

    if (_handle) {
        g_system->getMixer()->setChannelBalance(*_handle, _balance);
        g_system->getMixer()->setChannelVolume(*_handle, (byte)getEffectiveVolume());
    }
}

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 Lua_V2::WalkActorToAvoiding() {
	lua_Object actorObj = lua_getparam(1);
	lua_Object actor2Obj = lua_getparam(2);
	lua_Object xObj = lua_getparam(3);
	lua_Object yObj = lua_getparam(4);
	lua_Object zObj = lua_getparam(5);

	if (!lua_isuserdata(actorObj) || lua_tag(actorObj) != MKTAG('A','C','T','R'))
		return;

	if (!lua_isuserdata(actor2Obj) || lua_tag(actor2Obj) != MKTAG('A','C','T','R'))
		return;

	Math::Vector3d destVec;
	Actor *actor = getactor(actorObj);
	if (!lua_isnumber(xObj)) {
		if (!lua_isuserdata(xObj) || lua_tag(xObj) != MKTAG('A','C','T','R'))
			return;
		Actor *destActor = getactor(xObj);
		destVec = destActor->getPos();
	} else {
		float x = lua_getnumber(xObj);
		float y = lua_getnumber(yObj);
		float z = lua_getnumber(zObj);
		destVec.set(x, y, z);
	}

	// TODO: Make this actually avoid the second actor

	actor->walkTo(destVec);
}

void Lua_V2::GetActorPuckVector() {
	lua_Object actorObj = lua_getparam(1);
	lua_Object addObj = lua_getparam(2);

	if (!lua_isuserdata(actorObj) || lua_tag(actorObj) != MKTAG('A','C','T','R')) {
		lua_pushnil();
		return;
	}

	Actor *actor = getactor(actorObj);
	// Note: The wear chore of dumbshadow.cos is only started from Lua if
	// GetActorPuckVector returns a non-nil value. The original engine seems
	// to return nil for all actors that have never followed walkboxes.
	if (!actor || !actor->hasFollowedBoxes()) {
		lua_pushnil();
		return;
	}

	Math::Vector3d result = actor->getPuckVector();
	if (!lua_isnil(addObj))
		result += actor->getPos();

	lua_pushnumber(result.x());
	lua_pushnumber(result.y());
	lua_pushnumber(result.z());
}

// Find the closest point on the walkplane to the given point
Math::Vector3d Sector::getClosestPoint(const Math::Vector3d &point) const {
	// First try to project to the plane
	Math::Vector3d p2 = getProjectionToPlane(point);
	if (isPointInSector(p2))
		return p2;

	// Now try to project to some edge
	for (int i = 0; i < _numVertices; i++) {
		Math::Vector3d edge = _vertices[i + 1] - _vertices[i];
		Math::Vector3d delta = point - _vertices[i];
		float scalar = Math::Vector3d::dotProduct(delta, edge) / Math::Vector3d::dotProduct(edge, edge);
		Math::Vector3d cross = Math::Vector3d::crossProduct(delta, edge);
		if (scalar >= 0 && scalar <= 1 && cross.dotProduct(_normal) > 0)
			// That last test is just whether the z-component
			// of delta cross edge is positive; we don't
			// want to return opposite edges.
			return _vertices[i] + scalar * edge;
	}

	// Otherwise, just find the closest vertex
	float minDist = (point - _vertices[0]).getMagnitude();
	int index = 0;
	for (int i = 1; i < _numVertices; i++) {
		float currDist = (point - _vertices[i]).getMagnitude();
		if (currDist < minDist) {
			minDist = currDist;
			index = i;
		}
	}
	return _vertices[index];
}

Math::Vector3d Sector::getProjectionToPuckVector(const Math::Vector3d &v) const {
	if (_normal.z() == 0)
		error("Trying to walk along vertical plane");

	Math::Vector3d result = v;
	result.z() -= Math::Vector3d::dotProduct(_normal, v) / _normal.z();
	return result;
}

Command *Command::opIsItemNearPlace(const ResourceReference &itemRef, const ResourceReference &positionRef, int32 testDistance) {
	FloorPositionedItem *item = itemRef.resolve<FloorPositionedItem>();

	Math::Vector3d itemPostion = item->getPosition3D();
	Math::Vector3d testPosition = getObjectPosition(positionRef);

	return nextCommandIf(itemPostion.getDistanceTo(testPosition) < testDistance);
}

bool Sector::isPointInSector(const Math::Vector3d &point) const {
	// Calculate the distance of the point from the plane of the sector.
	// Return false if it isn't within a margin.
	if (_height < 9000.f) { // No need to check when height is 9999.
		// The plane has equation ax + by + cz + d = 0
		float a = _normal.x();
		float b = _normal.y();
		float c = _normal.z();
		float d = -_vertices[0].x() * a - _vertices[0].y() * b - _vertices[0].z() * c;

		float dist = (a * point.x() + b * point.y() + c * point.z() + d) /
					sqrt(a * a + b * b + c * c);
		// dist is positive if it is above the plain, negative if it is
		// below and 0 if it is on the plane.

		if (fabsf(dist) > _height + 0.01) // Add an error margin
			return false;
	}

	// On the plane, so check if it is inside the polygon.
	for (int i = 0; i < _numVertices; i++) {
		Math::Vector3d edge = _vertices[i + 1] - _vertices[i];
		Math::Vector3d delta = point - _vertices[i];
		if (edge.x() * delta.y() < edge.y() * delta.x())
			return false;
	}
	return true;
}

static Math::Vector3d directionToVector(float pitch, float heading) {
	Math::Vector3d v;

	float radHeading = Common::deg2rad(heading);
	float radPitch = Common::deg2rad(pitch);

	v.setValue(0, cos(radPitch) * cos(radHeading));
	v.setValue(1, sin(radPitch));
	v.setValue(2, cos(radPitch) * sin(radHeading));

	return v;
}

Math::Angle Actor::getYawTo(Actor *a) const {
	Math::Vector3d forwardVec = getSimplePuckVector();
	Math::Vector3d delta = a->getPos() - _pos;

	if (g_grim->getGameType() == GType_MONKEY4) {
		delta.y() = 0;
	} else {
		delta.z() = 0;
	}

	return Math::Vector3d::angle(forwardVec, delta);
}

float Sector::distanceToPoint(const Math::Vector3d &point) const {
	// The plane has equation ax + by + cz + d = 0
	float a = _normal.x();
	float b = _normal.y();
	float c = _normal.z();
	float d = -_vertices[0].x() * a - _vertices[0].y() * b - _vertices[0].z() * c;

	// dist is positive if it is above the plain, negative if it is
	// below and 0 if it is on the plane.
	float dist = (a * point.x() + b * point.y() + c * point.z() + d);
	dist /= sqrt(a * a + b * b + c * c);
	return dist;
}

Math::Vector3d Actor::getTangentPos(const Math::Vector3d &pos, const Math::Vector3d &dest) const {
	if (_collisionMode == CollisionOff) {
		return dest;
	}

	Model *model = getCurrentCostume()->getModel();
	Math::Vector3d p = _pos + model->_insertOffset;
	float size = model->_radius * _collisionScale;

	Math::Vector2d p1(pos.x(), pos.y());
	Math::Vector2d p2(dest.x(), dest.y());
	Math::Segment2d segment(p1, p2);

	// TODO: collision with Box
// 	if (_collisionMode == CollisionSphere) {
		Math::Vector2d center(p.x(), p.y());

		Math::Vector2d inter;
		float distance = segment.getLine().getDistanceTo(center, &inter);

		if (distance < size && segment.containsPoint(inter)) {
			Math::Vector2d v(inter - center);
			v.normalize();
			v *= size;
			v += center;

			return Math::Vector3d(v.getX(), v.getY(), dest.z());
		}
// 	} else {

// 	}

	return dest;
}