| 33 | { |
| 34 | template < geode::index_t dimension > |
| 35 | std::optional< geode::Frame< dimension > > compute_inverse( |
| 36 | geode::Frame< dimension > frame ) |
| 37 | { |
| 38 | std::optional< geode::Frame< dimension > > result{ std::in_place }; |
| 39 | for( const auto i : geode::LRange{ dimension } ) |
| 40 | { |
| 41 | auto value = frame.direction( i ).value( i ); |
| 42 | auto index = i; |
| 43 | for( const auto j : geode::LRange{ i + 1, dimension } ) |
| 44 | { |
| 45 | const auto& cur_value = frame.direction( j ).value( i ); |
| 46 | if( std::fabs( cur_value ) > std::fabs( value ) ) |
| 47 | { |
| 48 | index = j; |
| 49 | value = cur_value; |
| 50 | } |
| 51 | } |
| 52 | |
| 53 | if( value == 0 ) |
| 54 | { |
| 55 | return std::nullopt; |
| 56 | } |
| 57 | |
| 58 | if( index != i ) |
| 59 | { |
| 60 | auto temp_result = result->direction( i ); |
| 61 | result->set_direction( i, result->direction( index ) ); |
| 62 | result->set_direction( index, std::move( temp_result ) ); |
| 63 | auto temp_frame = frame.direction( i ); |
| 64 | frame.set_direction( i, frame.direction( index ) ); |
| 65 | frame.set_direction( index, std::move( temp_frame ) ); |
| 66 | } |
| 67 | |
| 68 | frame.set_direction( i, frame.direction( i ) / value ); |
| 69 | result->set_direction( i, result->direction( i ) / value ); |
| 70 | |
| 71 | for( const auto j : geode::LRange{ dimension } ) |
| 72 | { |
| 73 | if( j == i ) |
| 74 | { |
| 75 | continue; |
| 76 | } |
| 77 | const auto scale = frame.direction( j ).value( i ); |
| 78 | frame.set_direction( |
| 79 | j, frame.direction( j ) - frame.direction( i ) * scale ); |
| 80 | result->set_direction( j, |
| 81 | result->direction( j ) - result->direction( i ) * scale ); |
| 82 | } |
| 83 | } |
| 84 | |
| 85 | return result; |
| 86 | } |
| 87 | } // namespace |
| 88 | |
| 89 | namespace geode |
no test coverage detected