Пример #1
0
bool FlightTaskFailsafe::update()
{
	if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) {
		// stay at current position setpoint
		_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
		_thrust_setpoint(0) = _thrust_setpoint(1) = 0.0f;

	} else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) {
		// don't move along xy
		_position_setpoint(0) = _position_setpoint(1) = NAN;
		_thrust_setpoint(0) = _thrust_setpoint(1) = NAN;
	}

	if (PX4_ISFINITE(_position(2))) {
		// stay at current altitude setpoint
		_velocity_setpoint(2) = 0.0f;
		_thrust_setpoint(2) = NAN;

	} else if (PX4_ISFINITE(_velocity(2))) {
		// land with landspeed
		_velocity_setpoint(2) = MPC_LAND_SPEED.get();
		_position_setpoint(2) = NAN;
		_thrust_setpoint(2) = NAN;
	}

	return true;

}
Пример #2
0
inline Parse *
pop(ParserState *S, s32 end)
{
    hale_assert(S->stack->count != 0);

	flush(S, _position(S) + end);
    ParserStackItem *item = S->stack->pop();
    return item->parse;
}
Пример #3
0
inline void
push(ParserState *S, memi token_id, Parse *parse, s32 begin)
{
	flush(S, _position(S) + begin);

    ParserStackItem *item = S->stack->push(1, 16);
    *item = {};
    item->parse = parse;
    item->token_id = token_id;
}
Пример #4
0
Token *
token_add(ParserState *S, memi token_id, s32 begin, s32 end)
{
    // TODO: Assert when the new token is overlapping with the token already in the tokens list.
	hale_assert_input(end >= begin);
	memi p = _position(S);
	hale_assert((s64)p >= (s64)begin);

	flush(S, p + begin);

    Token *token = S->tokens->push(1, 16);
	token->id = token_id;
    hale_assert_debug(((p+begin) & ~(memi)0xFFFF) == 0);
    hale_assert_debug(((p+end) & ~(memi)0xFFFF) == 0);
    token->begin = p + begin;
	token->end = p + end;

	return token;
}
Пример #5
0
hale_internal
HALE_DOCUMENT_PARSER_PARSE(_parse)
{
    S->it_begin = begin;
    S->it_end   = end;
    S->it       = begin;
    S->tokens   = tokens;
    S->stack    = stack;

    S->tokens->count = 0;
    if (S->stack->count == 0)
    {
        push(S, Root, _root, 0);
    }
//    else
//    {
//        S->token_stack.count = 0;

//        memi *p = S->token_stack.push(S->stack->count, 16);
//        for (memi i = 0; i < S->stack->count; i++) {
//            *p++ = token_add(S, S->stack->e[i].token_id, 0, HALE_S32_MAX) - S->tokens->e;
//        }
//    }

    Token *token;
    ParserStackItem *stack_top;
    Parse *parse;
    for (;;)
    {
        stack_top = top(S);
        if (stack_top->parse(S) == 0)
        {
            if (S->it == S->it_end) {
                break;
            }
            S->it++;
        }
    }

	flush(S, _position(S));
}
Пример #6
0
bool FlightTask::_evaluateVehicleLocalPosition()
{
	if ((_time_stamp_current - _sub_vehicle_local_position->get().timestamp) < _timeout) {

		// position
		if (_sub_vehicle_local_position->get().xy_valid) {
			_position(0) = _sub_vehicle_local_position->get().x;
			_position(1) = _sub_vehicle_local_position->get().y;

		} else {
			_position(0) = _position(1) = NAN;
		}

		if (_sub_vehicle_local_position->get().z_valid) {
			_position(2) = _sub_vehicle_local_position->get().z;

		} else {
			_position(2) = NAN;
		}

		// velocity
		if (_sub_vehicle_local_position->get().v_xy_valid) {
			_velocity(0) = _sub_vehicle_local_position->get().vx;
			_velocity(1) = _sub_vehicle_local_position->get().vy;

		} else {
			_velocity(0) = _velocity(1) = NAN;
		}

		if (_sub_vehicle_local_position->get().v_z_valid) {
			_velocity(2) = _sub_vehicle_local_position->get().vz;

		} else {
			_velocity(2) = NAN;
		}

		// yaw
		_yaw = _sub_vehicle_local_position->get().yaw;

		// distance to bottom
		_dist_to_bottom = NAN;

		if (_sub_vehicle_local_position->get().dist_bottom_valid
		    && PX4_ISFINITE(_sub_vehicle_local_position->get().dist_bottom)) {
			_dist_to_bottom =  _sub_vehicle_local_position->get().dist_bottom;
		}

		// global frame reference coordinates to enable conversions
		if (_sub_vehicle_local_position->get().xy_global && _sub_vehicle_local_position->get().z_global) {
			globallocalconverter_init(_sub_vehicle_local_position->get().ref_lat, _sub_vehicle_local_position->get().ref_lon,
						  _sub_vehicle_local_position->get().ref_alt, _sub_vehicle_local_position->get().ref_timestamp);
		}

		// We don't check here if states are valid or not.
		// Validity checks are done in the sub-classes.
		return true;

	} else {
		_resetSetpoints();
		return false;
	}
}
Пример #7
0
void JetEngineEmitter::EmitFromTag(const CppContextObject* object, const uitbc::ChunkyClass::Tag& tag, float frame_time) {
	bool particles_enabled;
	v_get(particles_enabled, =, UiCure::GetSettings(), kRtvarUi3DEnableparticles, false);
	if (!particles_enabled) {
		return;
	}

	enum FloatValue {
		kFvStartR = 0,
		kFvStartG,
		kFvStartB,
		kFvEndR,
		kFvEndG,
		kFvEndB,
		kFvX,
		kFvY,
		kFvZ,
		kFvRadiusX,
		kFvRadiusY,
		kFvRadiusZ,
		kFvScaleX,
		kFvScaleY,
		kFvScaleZ,
		kFvDirectionX,
		kFvDirectionY,
		kFvDirectionZ,
		kFvDensity,
		kFvOpacity,
		kFvOvershootOpacity,
		kFvOvershootCutoffDot,
		kFvOvershootDistanceUpscale,
		kFvOvershootEngineFactorBase,
		kFvCount
	};
	if (tag.float_value_list_.size() != kFvCount ||
		tag.string_value_list_.size() != 0 ||
		tag.body_index_list_.size() != 0 ||
		tag.engine_index_list_.size() != 1 ||
		tag.mesh_index_list_.size() < 1) {
		log_.Errorf("The fire tag '%s' has the wrong # of parameters.", tag.tag_name_.c_str());
		deb_assert(false);
		return;
	}
	const int engine_index = tag.engine_index_list_[0];
	if (engine_index >= object->GetPhysics()->GetEngineCount()) {
		return;
	}
	const tbc::PhysicsEngine* engine = object->GetPhysics()->GetEngine(engine_index);
	const float throttle_up_speed = Math::GetIterateLerpTime(tag.float_value_list_[kFvOvershootEngineFactorBase]*0.5f, frame_time);
	const float throttle_down_speed = Math::GetIterateLerpTime(tag.float_value_list_[kFvOvershootEngineFactorBase], frame_time);
	const float engine_throttle = engine->GetLerpThrottle(throttle_up_speed, throttle_down_speed, true);
	const quat orientation = object->GetOrientation();
	vec3 _radius(tag.float_value_list_[kFvRadiusX], tag.float_value_list_[kFvRadiusY], tag.float_value_list_[kFvRadiusZ]);
	_radius.x *= Math::Lerp(1.0f, tag.float_value_list_[kFvScaleX], engine_throttle);
	_radius.y *= Math::Lerp(1.0f, tag.float_value_list_[kFvScaleY], engine_throttle);
	_radius.z *= Math::Lerp(1.0f, tag.float_value_list_[kFvScaleZ], engine_throttle);
	vec3 _position(tag.float_value_list_[kFvX], tag.float_value_list_[kFvY], tag.float_value_list_[kFvZ]);
	_position = orientation * _position;
	const vec3 _color(tag.float_value_list_[kFvEndR], tag.float_value_list_[kFvEndB], tag.float_value_list_[kFvEndB]);

	bool create_particle = false;
	const float density = tag.float_value_list_[kFvDensity];
	float exhaust_intensity;
	v_get(exhaust_intensity, =(float), UiCure::GetSettings(), kRtvarUi3DExhaustintensity, 1.0);
	interleave_timeout_ -= Math::Lerp(0.3f, 1.0f, engine_throttle) * exhaust_intensity * frame_time;
	if (interleave_timeout_ <= 0) {	// Release particle this frame?
		create_particle = true;
		interleave_timeout_ = 0.05f / density;
	} else {
		create_particle = false;
	}

	const float dx = tag.float_value_list_[kFvRadiusX];
	const float dy = tag.float_value_list_[kFvRadiusY];
	const float dz = tag.float_value_list_[kFvRadiusZ];
	const vec3 start_color(tag.float_value_list_[kFvStartR], tag.float_value_list_[kFvStartB], tag.float_value_list_[kFvStartB]);
	const float _opacity = tag.float_value_list_[kFvOpacity];
	const vec3 direction = orientation * vec3(tag.float_value_list_[kFvDirectionX], tag.float_value_list_[kFvDirectionY], tag.float_value_list_[kFvDirectionZ]);
	const vec3 velocity = direction + object->GetVelocity();
	uitbc::ParticleRenderer* particle_renderer = (uitbc::ParticleRenderer*)ui_manager_->GetRenderer()->GetDynamicRenderer("particle");
	const float particle_time = density;
	float particle_size;	// Pick second size.
	if (dx > dy && dy > dz) {
		particle_size = dy;
	} else if (dy > dx && dx > dz) {
		particle_size = dx;
	} else {
		particle_size = dz;
	}
	particle_size *= 0.2f;

	const float _distance_scale_factor = tag.float_value_list_[kFvOvershootDistanceUpscale];
	for (size_t y = 0; y < tag.mesh_index_list_.size(); ++y) {
		tbc::GeometryBase* mesh = object->GetMesh(tag.mesh_index_list_[y]);
		if (mesh) {
			int phys_index = -1;
			str mesh_name;
			xform transform;
			float mesh_scale;
			((uitbc::ChunkyClass*)object->GetClass())->GetMesh(tag.mesh_index_list_[y], phys_index, mesh_name, transform, mesh_scale);
			transform = mesh->GetBaseTransformation() * transform;
			vec3 mesh_pos = transform.GetPosition() + _position;

			const vec3 cam_distance = mesh_pos - ui_manager_->GetRenderer()->GetCameraTransformation().GetPosition();
			const float distance = cam_distance.GetLength();
			const vec3 cam_direction(cam_distance / distance);
			float overshoot_factor = -(cam_direction*direction);
			if (overshoot_factor > tag.float_value_list_[kFvOvershootCutoffDot]) {
				overshoot_factor = Math::Lerp(overshoot_factor*0.5f, overshoot_factor, engine_throttle);
				const float opacity = (overshoot_factor+0.6f) * tag.float_value_list_[kFvOvershootOpacity];
				DrawOvershoot(mesh_pos, _distance_scale_factor*distance, _radius, _color, opacity, cam_direction);
			}

			if (create_particle) {
				const float sx = Random::Normal(0.0f, dx*0.5f, -dx, +dx);
				const float sy = Random::Normal(0.0f, dy*0.5f, -dy, +dy);
				const float sz = Random::Normal(0.0f, dz*0.5f, -dz, +dz);
				mesh_pos += orientation * vec3(sx, sy, sz);
				particle_renderer->CreateGlow(particle_time, particle_size, start_color, _color, _opacity, mesh_pos, velocity);
			}
		}
	}
}
void NodeManager::updateTransforms(const NodeRefArray& p_Nodes)
{
  for (uint32_t nodeIdx = 0u; nodeIdx < p_Nodes.size(); ++nodeIdx)
  {
    NodeRef nodeRef = p_Nodes[nodeIdx];
    NodeRef parentNodeRef = _parent(nodeRef);

    if (!parentNodeRef.isValid())
    {
      _worldPosition(nodeRef) = _position(nodeRef);
      _worldOrientation(nodeRef) = _orientation(nodeRef);
      _worldSize(nodeRef) = _size(nodeRef);
    }
    else
    {
      const glm::vec3& parentPos = _worldPosition(parentNodeRef);
      const glm::quat& parentOrient = _worldOrientation(parentNodeRef);
      const glm::vec3& parentSize = _worldSize(parentNodeRef);

      const glm::vec3& localPos = _position(nodeRef);
      const glm::quat& localOrient = _orientation(nodeRef);
      const glm::vec3& localSize = _size(nodeRef);

      const glm::vec3 worldPos = parentPos + (parentOrient * localPos);
      const glm::quat worldOrient = parentOrient * localOrient;
      const glm::vec3 worldSize = parentSize * localSize;

      _worldPosition(nodeRef) = worldPos;
      _worldOrientation(nodeRef) = worldOrient;
      _worldSize(nodeRef) = worldSize;
    }

    glm::mat4 rot = glm::mat4_cast(NodeManager::_worldOrientation(nodeRef));
    glm::mat4 trans =
        glm::translate(glm::mat4(1.0f), NodeManager::_worldPosition(nodeRef));
    glm::mat4 scale =
        glm::scale(glm::mat4(1.0f), NodeManager::_worldSize(nodeRef));

    _worldMatrix(nodeRef) = trans * rot * scale;
    _inverseWorldMatrix(nodeRef) = glm::inverse(_worldMatrix(nodeRef));

    // Update AABB
    // TODO: Merge sub meshes
    Components::MeshRef meshCompRef =
        Components::MeshManager::getComponentForEntity(_entity(nodeRef));
    if (meshCompRef.isValid())
    {
      Name& meshName = Components::MeshManager::_descMeshName(meshCompRef);
      Resources::MeshRef meshRef =
          Resources::MeshManager::_getResourceByName(meshName);

      if (meshRef.isValid())
      {
        const uint32_t aabbCount =
            (uint32_t)Resources::MeshManager::_aabbPerSubMesh(meshRef).size();

        if (aabbCount > 0u)
        {
          _localAABB(nodeRef) =
              Resources::MeshManager::_aabbPerSubMesh(meshRef)[0u];
          _worldAABB(nodeRef) = _localAABB(nodeRef);
          Math::transformAABBAffine(_worldAABB(nodeRef), _worldMatrix(nodeRef));

          _worldBoundingSphere(nodeRef) = {
              Math::calcAABBCenter(_worldAABB(nodeRef)),
              glm::length(Math::calcAABBHalfExtent(_worldAABB(nodeRef)))};
        }
      }
    }
    else
    {
      _worldAABB(nodeRef) =
          Math::AABB(_worldPosition(nodeRef) - glm::vec3(0.5f),
                     _worldPosition(nodeRef) + glm::vec3(0.5f));
    }
  }
}
Пример #9
0
Vector2d Point::getPosition() const
{
	return _position();
}