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;
}
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;
}
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;
}
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;
}
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));
}
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;
}
}
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));
}
}
}
Vector2d Point::getPosition() const
{
return _position();
}