@ -19,186 +19,279 @@
# include "constraint.hpp"
# include "game/component/constraint-stack.hpp"
# include "game/component/constraints/constraints.hpp"
# include "game/component/transform.hpp"
namespace game {
namespace system {
constraint : : constraint ( entity : : registry & registry ) :
updatable ( registry )
{ }
{
registry . on_construct < component : : constraint_stack > ( ) . connect < & constraint : : on_constraint_stack_update > ( this ) ;
registry . on_update < component : : constraint_stack > ( ) . connect < & constraint : : on_constraint_stack_update > ( this ) ;
registry . on_destroy < component : : constraint_stack > ( ) . connect < & constraint : : on_constraint_stack_update > ( this ) ;
}
constraint : : ~ constraint ( )
{
registry . on_construct < component : : constraint_stack > ( ) . disconnect < & constraint : : on_constraint_stack_update > ( this ) ;
registry . on_update < component : : constraint_stack > ( ) . disconnect < & constraint : : on_constraint_stack_update > ( this ) ;
registry . on_destroy < component : : constraint_stack > ( ) . disconnect < & constraint : : on_constraint_stack_update > ( this ) ;
}
void constraint : : update ( double t , double dt )
{
// For each entity with transform and constraint stack components
registry . view < component : : transform , component : : constraint_stack > ( ) . each (
registry . view < component : : transform , component : : constraint_stack > ( ) . each
(
[ & ] ( entity : : id transform_eid , auto & transform , auto & stack )
{
// Init world-space transform
transform . world = transform . local ;
// Get entity ID of first constraint
entity : : id constraint_eid = stack . head ;
// Consecutively apply constraints
while ( constraint_eid ! = entt : : null )
while ( registry . valid ( constraint_eid ) )
{
// Invalid constraint
if ( ! registry . has < component : : constraint_stack_node > ( constraint_eid ) )
break ;
// Get constraint stack node of the constraint
const component : : constraint_stack_node * node = registry . try_get < component : : constraint_stack_node > ( constraint_eid ) ;
// Get constraint stack node of this constraint
const auto & node = registry . get < component : : constraint_stack_node > ( constraint_eid ) ;
// Abort if constraint is missing a constraint stack node
if ( ! node )
break ;
// Apply constraint if enabled
if ( node . active )
if ( node - > active )
handle_constraint ( transform , constraint_eid , static_cast < float > ( dt ) ) ;
// Get entity ID of next constraint
constraint_eid = node . next ;
// Get entity ID of next constraint in the stack
constraint_eid = node - > next ;
}
} ) ;
}
) ;
}
void constraint : : on_constraint_stack_update ( entity : : registry & registry , entity : : id constraint_stack_eid )
{
registry . sort < component : : constraint_stack >
(
[ ] ( const auto & lhs , const auto & rhs )
{
return lhs . priority < rhs . priority ;
}
) ;
}
void constraint : : handle_constraint ( component : : transform & transform , entity : : id constraint_eid , float dt )
{
if ( registry . has < component : : constraint : : copy_translation > ( constraint_eid ) )
handle_copy_translation_constraint ( transform , constraint_eid ) ;
else if ( registry . has < component : : constraint : : copy_rotation > ( constraint_eid ) )
handle_copy_rotation_constraint ( transform , constraint_eid ) ;
else if ( registry . has < component : : constraint : : copy_scale > ( constraint_eid ) )
handle_copy_scale_constraint ( transform , constraint_eid ) ;
else if ( registry . has < component : : constraint : : copy_transform > ( constraint_eid ) )
handle_copy_transform_constraint ( transform , constraint_eid ) ;
else if ( registry . has < component : : constraint : : track_to > ( constraint_eid ) )
handle_track_to_constraint ( transform , constraint_eid ) ;
else if ( registry . has < component : : constraint : : three_dof > ( constraint_eid ) )
handle_three_dof_constraint ( transform , constraint_eid ) ;
else if ( registry . has < component : : constraint : : spring_to > ( constraint_eid ) )
handle_spring_to_constraint ( transform , constraint_eid , dt ) ;
}
void constraint : : handle_copy_translation_constraint ( component : : transform & transform , entity : : id constraint_eid )
{
const auto & params = registry . get < component : : constraint : : copy_translation > ( constraint_eid ) ;
if ( this - > registry . has < component : : transform > ( params . target ) )
if ( auto constraint = registry . try_get < component : : constraint : : copy_translation > ( constraint_eid ) ; constraint )
handle_copy_translation_constraint ( transform , * constraint ) ;
else if ( auto constraint = registry . try_get < component : : constraint : : copy_rotation > ( constraint_eid ) ; constraint )
handle_copy_rotation_constraint ( transform , * constraint ) ;
else if ( auto constraint = registry . try_get < component : : constraint : : copy_scale > ( constraint_eid ) ; constraint )
handle_copy_scale_constraint ( transform , * constraint ) ;
else if ( auto constraint = registry . try_get < component : : constraint : : copy_transform > ( constraint_eid ) ; constraint )
handle_copy_transform_constraint ( transform , * constraint ) ;
else if ( auto constraint = registry . try_get < component : : constraint : : track_to > ( constraint_eid ) ; constraint )
handle_track_to_constraint ( transform , * constraint ) ;
else if ( auto constraint = registry . try_get < component : : constraint : : three_dof > ( constraint_eid ) ; constraint )
handle_three_dof_constraint ( transform , * constraint ) ;
else if ( auto constraint = registry . try_get < component : : constraint : : pivot > ( constraint_eid ) ; constraint )
handle_pivot_constraint ( transform , * constraint ) ;
else if ( auto constraint = registry . try_get < component : : constraint : : child_of > ( constraint_eid ) ; constraint )
handle_child_of_constraint ( transform , * constraint ) ;
else if ( auto constraint = registry . try_get < component : : constraint : : spring_to > ( constraint_eid ) ; constraint )
handle_spring_to_constraint ( transform , * constraint , dt ) ;
else if ( auto constraint = registry . try_get < component : : constraint : : spring_translation > ( constraint_eid ) ; constraint )
handle_spring_translation_constraint ( transform , * constraint , dt ) ;
else if ( auto constraint = registry . try_get < component : : constraint : : spring_rotation > ( constraint_eid ) ; constraint )
handle_spring_rotation_constraint ( transform , * constraint , dt ) ;
}
void constraint : : handle_child_of_constraint ( component : : transform & transform , const component : : constraint : : child_of & constraint )
{
if ( registry . valid ( constraint . target ) )
{
const auto & target_translation = registry . get < component : : transform > ( params . target ) . world . translation ;
if ( params . offset )
const component : : transform * target_transform = registry . try_get < component : : transform > ( constraint . target ) ;
if ( target_transform )
{
if ( params . copy_x )
transform . world . translation . x + = ( params . invert_x ) ? - target_translation . x : target_translation . x ;
if ( params . copy_y )
transform . world . translation . y + = ( params . invert_y ) ? - target_translation . y : target_translation . y ;
if ( params . copy_z )
transform . world . translation . z + = ( params . invert_z ) ? - target_translation . z : target_translation . z ;
transform . world = target_transform - > world * transform . world ;
}
else
}
}
void constraint : : handle_copy_rotation_constraint ( component : : transform & transform , const component : : constraint : : copy_rotation & constraint )
{
if ( registry . valid ( constraint . target ) )
{
const component : : transform * target_transform = registry . try_get < component : : transform > ( constraint . target ) ;
if ( target_transform )
{
if ( params . copy_x )
transform . world . translation . x = ( params . invert_x ) ? - target_translation . x : target_translation . x ;
if ( params . copy_y )
transform . world . translation . y = ( params . invert_y ) ? - target_translation . y : target_translation . y ;
if ( params . copy_z )
transform . world . translation . z = ( params . invert_z ) ? - target_translation . z : target_translation . z ;
transform . world . rotation = target_transform - > world . rotation ;
}
}
}
void constraint : : handle_copy_rotation _constraint ( component : : transform & transform , entity : : id constraint_eid )
void constraint : : handle_copy_scale_constraint ( component : : transform & transform , const component : : constraint : : copy_scale & constraint )
{
const auto & params = registry . get < component : : constraint : : copy_rotation > ( constraint_eid ) ;
if ( this - > registry . has < component : : transform > ( params . target ) )
if ( registry . valid ( constraint . target ) )
{
const auto & target_rotation = registry . get < component : : transform > ( params . target ) . world . rotation ;
transform . world . rotation = target_rotation ;
const component : : transform * target_transform = registry . try_get < component : : transform > ( constraint . target ) ;
if ( target_transform )
{
const auto & target_scale = target_transform - > world . scale ;
if ( constraint . copy_x )
transform . world . scale . x = target_scale . x ;
if ( constraint . copy_y )
transform . world . scale . y = target_scale . y ;
if ( constraint . copy_z )
transform . world . scale . z = target_scale . z ;
}
}
}
void constraint : : handle_copy_scale_constraint ( component : : transform & transform , entity : : id constraint_eid )
void constraint : : handle_copy_transform _constraint ( component : : transform & transform , const compon ent: : constra in t: : copy_transform & constraint )
{
const auto & params = registry . get < component : : constraint : : copy_scale > ( constraint_eid ) ;
if ( this - > registry . has < component : : transform > ( params . target ) )
if ( registry . valid ( constraint . target ) )
{
const auto & target_scale = registry . get < component : : transform > ( params . target ) . world . scale ;
if ( params . copy_x )
transform . world . scale . x = target_scale . x ;
if ( params . copy_y )
transform . world . scale . y = target_scale . y ;
if ( params . copy_z )
transform . world . scale . z = target_scale . z ;
const component : : transform * target_transform = registry . try_get < component : : transform > ( constraint . target ) ;
if ( target_transform )
{
transform . world = target_transform - > world ;
}
}
}
void constraint : : handle_copy_transform _constraint ( component : : transform & transform , entity : : id constraint_eid )
void constraint : : handle_copy_translation _constraint ( component : : transform & transform , const compon ent: : constra in t: : copy_translation & constraint )
{
const auto & params = registry . get < component : : constraint : : copy_transform > ( constraint_eid ) ;
if ( this - > registry . has < component : : transform > ( params . target ) )
if ( registry . valid ( constraint . target ) )
{
const auto & target_transform = registry . get < component : : transform > ( params . target ) . world ;
transform . world = target_transform ;
const component : : transform * target_transform = registry . try_get < component : : transform > ( constraint . target ) ;
if ( target_transform )
{
const auto & target_translation = target_transform - > world . translation ;
if ( constraint . offset )
{
if ( constraint . copy_x )
transform . world . translation . x + = ( constraint . invert_x ) ? - target_translation . x : target_translation . x ;
if ( constraint . copy_y )
transform . world . translation . y + = ( constraint . invert_y ) ? - target_translation . y : target_translation . y ;
if ( constraint . copy_z )
transform . world . translation . z + = ( constraint . invert_z ) ? - target_translation . z : target_translation . z ;
}
else
{
if ( constraint . copy_x )
transform . world . translation . x = ( constraint . invert_x ) ? - target_translation . x : target_translation . x ;
if ( constraint . copy_y )
transform . world . translation . y = ( constraint . invert_y ) ? - target_translation . y : target_translation . y ;
if ( constraint . copy_z )
transform . world . translation . z = ( constraint . invert_z ) ? - target_translation . z : target_translation . z ;
}
}
}
}
void constraint : : handle_track_to_constraint ( component : : transform & transform , entity : : id constraint_eid )
void constraint : : handle_pivo t_constraint ( component : : transform & transform , const compon ent: : constra in t: : pivot & constraint )
{
const auto & params = registry . get < component : : constraint : : track_to > ( constraint_eid ) ;
if ( this - > registry . has < component : : transform > ( params . target ) )
if ( registry . valid ( constraint . target ) )
{
const auto & target_transform = registry . get < component : : transform > ( params . target ) . world ;
transform . world . rotation = math : : look_rotation ( math : : normalize ( math : : sub ( target_transform . translation , transform . world . translation ) ) , params . up ) ;
const component : : transform * target_transform = registry . try_get < component : : transform > ( constraint . target ) ;
if ( target_transform )
{
// Get pivot center point
const float3 pivot_center = target_transform - > world . translation + constraint . offset ;
// Pivot translation
transform . world . translation = pivot_center + transform . world . rotation * ( transform . world . translation - pivot_center ) ;
}
}
}
void constraint : : handle_three_dof_constraint ( component : : transform & transform , entity : : id constraint_eid )
void constraint : : handle_spring_rotation _constraint ( component : : transform & transform , compon ent: : constra in t: : spring_rotation & constraint , float dt )
{
const auto & params = registry . get < component : : constraint : : three_dof > ( constraint_eid ) ;
// Solve yaw, pitch, and roll angle spring
solve_numeric_spring < float3 , float > ( constraint . spring , dt ) ;
// Build yaw, pitch, and roll quaternions
const math : : quaternion < float > yaw = math : : angle_axis ( constraint . spring . x0 [ 0 ] , { 0.0f , 1.0f , 0.0f } ) ;
const math : : quaternion < float > pitch = math : : angle_axis ( constraint . spring . x0 [ 1 ] , { - 1.0f , 0.0f , 0.0f } ) ;
const math : : quaternion < float > roll = math : : angle_axis ( constraint . spring . x0 [ 2 ] , { 0.0f , 0.0f , - 1.0f } ) ;
const math : : quaternion < float > yaw = math : : angle_axis ( params . yaw , { 0.0f , 1.0f , 0.0f } ) ;
const math : : quaternion < float > pitch = math : : angle_axis ( params . pitch , { - 1.0f , 0.0f , 0.0f } ) ;
const math : : quaternion < float > roll = math : : angle_axis ( params . roll , { 0.0f , 0.0f , - 1.0f } ) ;
transform . local . rotation = math : : normalize ( yaw * pitch * roll ) ;
// Update transform rotation
transform . world . rotation = math : : normalize ( yaw * pitch * roll ) ;
}
void constraint : : handle_spring_to_constraint ( component : : transform & transform , entity : : id constraint_eid , float dt )
void constraint : : handle_spring_to_constraint ( component : : transform & transform , compon ent: : constra in t: : spring_to & constraint , float dt )
{
auto & params = registry . get < component : : constraint : : spring_to > ( constraint_eid ) ;
if ( this - > registry . has < component : : transform > ( params . target ) )
if ( registry . valid ( constraint . target ) )
{
// Get transform of target entity
const auto & target_transform = registry . get < component : : transform > ( params . target ) . world ;
// Spring translation
if ( params . spring_translation )
const component : : transform * target_transform = registry . try_get < component : : transform > ( constraint . target ) ;
if ( target_transform )
{
// Update translation spring target
params . translation . x1 = target_transform . translation ;
// Solve translation spring
solve_numeric_spring < float3 , float > ( params . translation , dt ) ;
// Spring translation
if ( constraint . spring_translation )
{
// Update translation spring target
constraint . translation . x1 = target_transform - > world . translation ;
// Solve translation spring
solve_numeric_spring < float3 , float > ( constraint . translation , dt ) ;
// Update transform translation
transform . world . translation = constraint . translation . x0 ;
}
// Update transform translation
transform . world . translation = params . translation . x0 ;
// Spring rotation
if ( constraint . spring_rotation )
{
// Update rotation spring target
constraint . rotation . x1 =
{
target_transform - > world . rotation . w ,
target_transform - > world . rotation . x ,
target_transform - > world . rotation . y ,
target_transform - > world . rotation . z
} ;
// Solve rotation spring
solve_numeric_spring < float4 , float > ( constraint . rotation , dt ) ;
// Update transform rotation
transform . world . rotation = math : : normalize ( math : : quaternion < float > { constraint . rotation . x0 [ 0 ] , constraint . rotation . x0 [ 1 ] , constraint . rotation . x0 [ 2 ] , constraint . rotation . x0 [ 3 ] } ) ;
}
}
// Spring rotation
if ( params . spring_rotation )
}
}
void constraint : : handle_spring_translation_constraint ( component : : transform & transform , component : : constraint : : spring_translation & constraint , float dt )
{
// Solve translation spring
solve_numeric_spring < float3 , float > ( constraint . spring , dt ) ;
// Update transform translation
transform . world . translation = constraint . spring . x0 ;
}
void constraint : : handle_three_dof_constraint ( component : : transform & transform , const component : : constraint : : three_dof & constraint )
{
const math : : quaternion < float > yaw = math : : angle_axis ( constraint . yaw , { 0.0f , 1.0f , 0.0f } ) ;
const math : : quaternion < float > pitch = math : : angle_axis ( constraint . pitch , { - 1.0f , 0.0f , 0.0f } ) ;
const math : : quaternion < float > roll = math : : angle_axis ( constraint . roll , { 0.0f , 0.0f , - 1.0f } ) ;
transform . world . rotation = math : : normalize ( yaw * pitch * roll ) ;
}
void constraint : : handle_track_to_constraint ( component : : transform & transform , const component : : constraint : : track_to & constraint )
{
if ( registry . valid ( constraint . target ) )
{
const component : : transform * target_transform = registry . try_get < component : : transform > ( constraint . target ) ;
if ( target_transform )
{
// Update rotation spring target
params . rotation . x1 = { target_transform . rotation . w , target_transform . rotation . x , target_transform . rotation . y , target_transform . rotation . z } ;
// Solve rotation spring
solve_numeric_spring < float4 , float > ( params . rotation , dt ) ;
// Update transform rotation
transform . world . rotation = math : : normalize ( math : : quaternion < float > { params . rotation . x0 [ 0 ] , params . rotation . x0 [ 1 ] , params . rotation . x0 [ 2 ] , params . rotation . x0 [ 3 ] } ) ;
transform . world . rotation = math : : look_rotation ( math : : normalize ( math : : sub ( target_transform - > world . translation , transform . world . translation ) ) , constraint . up ) ;
}
}
}