; LLVM IR from erwincoumans/tiny-differentiable-simulator :: third_party/rbdl/src (Constraints)

; Unoptimized IR (clang -disable-llvm-passes):
target datalayout = "e-m:e-p270:32:32-p271:32:32-p272:64:64-i64:64-i128:128-f80:128-n8:16:32:64-S128"
target triple = "x86_64-unknown-linux-gnu"

%"class.std::basic_ostream" = type { ptr, %"class.std::basic_ios" }
%"class.std::basic_ios" = type { %"class.std::ios_base", ptr, i8, i8, ptr, ptr, ptr, ptr }
%"class.std::ios_base" = type { ptr, i64, i64, i32, i32, i32, ptr, %"struct.std::ios_base::_Words", [8 x %"struct.std::ios_base::_Words"], i32, ptr, %"class.std::locale" }
%"struct.std::ios_base::_Words" = type { ptr, i64 }
%"class.std::locale" = type { ptr }
%"class.Eigen::CwiseNullaryOp.177" = type { %"class.Eigen::internal::variable_if_dynamic", %"class.Eigen::internal::variable_if_dynamic", %"struct.Eigen::internal::scalar_constant_op" }
%"class.Eigen::internal::variable_if_dynamic" = type { i64 }
%"struct.Eigen::internal::scalar_constant_op" = type { double }
%"class.Eigen::CwiseNullaryOp.182" = type { %"class.Eigen::internal::variable_if_dynamic", [8 x i8], %"struct.Eigen::internal::scalar_constant_op" }
%"class.Eigen::Matrix.41" = type { %"class.Eigen::PlainObjectBase.42" }
%"class.Eigen::PlainObjectBase.42" = type { %"class.Eigen::DenseStorage.49" }
%"class.Eigen::DenseStorage.49" = type { ptr, i64, i64 }
%"class.Eigen::Matrix" = type { %"class.Eigen::PlainObjectBase" }
%"class.Eigen::PlainObjectBase" = type { %"class.Eigen::DenseStorage" }
%"class.Eigen::DenseStorage" = type { ptr, i64 }
%"class.Eigen::Block" = type { %"class.Eigen::BlockImpl" }
%"class.Eigen::BlockImpl" = type { %"class.Eigen::internal::BlockImpl_dense" }
%"class.Eigen::internal::BlockImpl_dense" = type { %"class.Eigen::MapBase", ptr, %"class.Eigen::internal::variable_if_dynamic", %"class.Eigen::internal::variable_if_dynamic", i64 }
%"class.Eigen::MapBase" = type { %"class.Eigen::MapBase.202" }
%"class.Eigen::MapBase.202" = type { ptr, %"class.Eigen::internal::variable_if_dynamic", %"class.Eigen::internal::variable_if_dynamic" }
%"class.Eigen::Transpose" = type { ptr }
%"class.Eigen::CwiseUnaryOp" = type <{ ptr, [8 x i8] }>
%"class.Eigen::Block.216" = type { %"class.Eigen::BlockImpl.217" }
%"class.Eigen::BlockImpl.217" = type { %"class.Eigen::internal::BlockImpl_dense.218" }
%"class.Eigen::internal::BlockImpl_dense.218" = type { %"class.Eigen::MapBase.219", ptr, %"class.Eigen::internal::variable_if_dynamic", %"class.Eigen::internal::variable_if_dynamic", i64 }
%"class.Eigen::MapBase.219" = type { %"class.Eigen::MapBase.220" }
%"class.Eigen::MapBase.220" = type { ptr, %"class.Eigen::internal::variable_if_dynamic", %"class.Eigen::internal::variable_if_dynamic" }
%"class.RigidBodyDynamics::Math::Quaternion" = type { %class.Vector4_t }
%class.Vector4_t = type { %"class.Eigen::Matrix.486" }
%"class.Eigen::Matrix.486" = type { %"class.Eigen::PlainObjectBase.487" }
%"class.Eigen::PlainObjectBase.487" = type { %"class.Eigen::DenseStorage.494" }
%"class.Eigen::DenseStorage.494" = type { %"struct.Eigen::internal::plain_array.495" }
%"struct.Eigen::internal::plain_array.495" = type { [4 x double] }
%class.Vector3_t = type { %"class.Eigen::Matrix.70" }
%"class.Eigen::Matrix.70" = type { %"class.Eigen::PlainObjectBase.71" }
%"class.Eigen::PlainObjectBase.71" = type { %"class.Eigen::DenseStorage.78" }
%"class.Eigen::DenseStorage.78" = type { %"struct.Eigen::internal::plain_array" }
%"struct.Eigen::internal::plain_array" = type { [3 x double] }
%"class.Eigen::Block.496" = type { %"class.Eigen::BlockImpl.497" }
%"class.Eigen::BlockImpl.497" = type { %"class.Eigen::internal::BlockImpl_dense.498" }
%"class.Eigen::internal::BlockImpl_dense.498" = type { %"class.Eigen::MapBase.base.508", ptr, %"class.Eigen::internal::variable_if_dynamic", [8 x i8], i64 }
%"class.Eigen::MapBase.base.508" = type { %"class.Eigen::MapBase.base.507" }
%"class.Eigen::MapBase.base.507" = type <{ ptr, [2 x i8] }>
%"struct.RigidBodyDynamics::Model" = type { %"class.std::vector.6", %"class.std::vector.6", %"class.std::vector.118", i32, i32, i32, i32, %class.Vector3_t, %"class.std::vector.27", %"class.std::vector.27", %"class.std::vector.123", %"class.std::vector.27", %"class.std::vector.27", %"class.std::vector.27", %"class.std::vector.6", %"class.std::vector.21", %"class.std::vector.6", %"class.std::vector.130", %"class.std::vector.130", %"class.std::vector.137", %"class.std::vector.16", %"class.std::vector.6", %"class.std::vector.142", %"class.std::vector.27", %"class.std::vector.60", %"class.std::vector.27", %"class.std::vector.27", %"class.Eigen::Matrix", %"class.Eigen::Matrix", %"class.std::vector.27", %"class.std::vector.147", %"class.std::vector.147", %"class.std::vector.27", %"class.std::vector.27", %"class.std::vector.21", %"class.std::vector.21", %"class.std::vector.154", i32, %"class.std::vector.161", %"class.std::map" }
%"class.std::vector.118" = type { %"struct.std::_Vector_base.119" }
%"struct.std::_Vector_base.119" = type { %"struct.std::_Vector_base<std::vector<unsigned int>, std::allocator<std::vector<unsigned int>>>::_Vector_impl" }
%"struct.std::_Vector_base<std::vector<unsigned int>, std::allocator<std::vector<unsigned int>>>::_Vector_impl" = type { %"struct.std::_Vector_base<std::vector<unsigned int>, std::allocator<std::vector<unsigned int>>>::_Vector_impl_data" }
%"struct.std::_Vector_base<std::vector<unsigned int>, std::allocator<std::vector<unsigned int>>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.123" = type { %"class.std::vector.124" }
%"class.std::vector.124" = type { %"struct.std::_Vector_base.125" }
%"struct.std::_Vector_base.125" = type { %"struct.std::_Vector_base<RigidBodyDynamics::Joint, Eigen::aligned_allocator<RigidBodyDynamics::Joint>>::_Vector_impl" }
%"struct.std::_Vector_base<RigidBodyDynamics::Joint, Eigen::aligned_allocator<RigidBodyDynamics::Joint>>::_Vector_impl" = type { %"struct.std::_Vector_base<RigidBodyDynamics::Joint, Eigen::aligned_allocator<RigidBodyDynamics::Joint>>::_Vector_impl_data" }
%"struct.std::_Vector_base<RigidBodyDynamics::Joint, Eigen::aligned_allocator<RigidBodyDynamics::Joint>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.130" = type { %"class.std::vector.131" }
%"class.std::vector.131" = type { %"struct.std::_Vector_base.132" }
%"struct.std::_Vector_base.132" = type { %"struct.std::_Vector_base<Eigen::Matrix<double, 6, 3>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 3>>>::_Vector_impl" }
%"struct.std::_Vector_base<Eigen::Matrix<double, 6, 3>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 3>>>::_Vector_impl" = type { %"struct.std::_Vector_base<Eigen::Matrix<double, 6, 3>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 3>>>::_Vector_impl_data" }
%"struct.std::_Vector_base<Eigen::Matrix<double, 6, 3>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 3>>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.137" = type { %"struct.std::_Vector_base.138" }
%"struct.std::_Vector_base.138" = type { %"struct.std::_Vector_base<Matrix3_t, std::allocator<Matrix3_t>>::_Vector_impl" }
%"struct.std::_Vector_base<Matrix3_t, std::allocator<Matrix3_t>>::_Vector_impl" = type { %"struct.std::_Vector_base<Matrix3_t, std::allocator<Matrix3_t>>::_Vector_impl_data" }
%"struct.std::_Vector_base<Matrix3_t, std::allocator<Matrix3_t>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.16" = type { %"struct.std::_Vector_base.17" }
%"struct.std::_Vector_base.17" = type { %"struct.std::_Vector_base<Vector3_t, std::allocator<Vector3_t>>::_Vector_impl" }
%"struct.std::_Vector_base<Vector3_t, std::allocator<Vector3_t>>::_Vector_impl" = type { %"struct.std::_Vector_base<Vector3_t, std::allocator<Vector3_t>>::_Vector_impl_data" }
%"struct.std::_Vector_base<Vector3_t, std::allocator<Vector3_t>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.6" = type { %"struct.std::_Vector_base.7" }
%"struct.std::_Vector_base.7" = type { %"struct.std::_Vector_base<unsigned int, std::allocator<unsigned int>>::_Vector_impl" }
%"struct.std::_Vector_base<unsigned int, std::allocator<unsigned int>>::_Vector_impl" = type { %"struct.std::_Vector_base<unsigned int, std::allocator<unsigned int>>::_Vector_impl_data" }
%"struct.std::_Vector_base<unsigned int, std::allocator<unsigned int>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.142" = type { %"struct.std::_Vector_base.143" }
%"struct.std::_Vector_base.143" = type { %"struct.std::_Vector_base<RigidBodyDynamics::CustomJoint *, std::allocator<RigidBodyDynamics::CustomJoint *>>::_Vector_impl" }
%"struct.std::_Vector_base<RigidBodyDynamics::CustomJoint *, std::allocator<RigidBodyDynamics::CustomJoint *>>::_Vector_impl" = type { %"struct.std::_Vector_base<RigidBodyDynamics::CustomJoint *, std::allocator<RigidBodyDynamics::CustomJoint *>>::_Vector_impl_data" }
%"struct.std::_Vector_base<RigidBodyDynamics::CustomJoint *, std::allocator<RigidBodyDynamics::CustomJoint *>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.60" = type { %"class.std::vector.61" }
%"class.std::vector.61" = type { %"struct.std::_Vector_base.62" }
%"struct.std::_Vector_base.62" = type { %"struct.std::_Vector_base<SpatialMatrix_t, Eigen::aligned_allocator<SpatialMatrix_t>>::_Vector_impl" }
%"struct.std::_Vector_base<SpatialMatrix_t, Eigen::aligned_allocator<SpatialMatrix_t>>::_Vector_impl" = type { %"struct.std::_Vector_base<SpatialMatrix_t, Eigen::aligned_allocator<SpatialMatrix_t>>::_Vector_impl_data" }
%"struct.std::_Vector_base<SpatialMatrix_t, Eigen::aligned_allocator<SpatialMatrix_t>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.147" = type { %"class.std::vector.148" }
%"class.std::vector.148" = type { %"struct.std::_Vector_base.149" }
%"struct.std::_Vector_base.149" = type { %"struct.std::_Vector_base<RigidBodyDynamics::Math::SpatialRigidBodyInertia, Eigen::aligned_allocator<RigidBodyDynamics::Math::SpatialRigidBodyInertia>>::_Vector_impl" }
%"struct.std::_Vector_base<RigidBodyDynamics::Math::SpatialRigidBodyInertia, Eigen::aligned_allocator<RigidBodyDynamics::Math::SpatialRigidBodyInertia>>::_Vector_impl" = type { %"struct.std::_Vector_base<RigidBodyDynamics::Math::SpatialRigidBodyInertia, Eigen::aligned_allocator<RigidBodyDynamics::Math::SpatialRigidBodyInertia>>::_Vector_impl_data" }
%"struct.std::_Vector_base<RigidBodyDynamics::Math::SpatialRigidBodyInertia, Eigen::aligned_allocator<RigidBodyDynamics::Math::SpatialRigidBodyInertia>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.27" = type { %"class.std::vector.28" }
%"class.std::vector.28" = type { %"struct.std::_Vector_base.29" }
%"struct.std::_Vector_base.29" = type { %"struct.std::_Vector_base<SpatialVector_t, Eigen::aligned_allocator<SpatialVector_t>>::_Vector_impl" }
%"struct.std::_Vector_base<SpatialVector_t, Eigen::aligned_allocator<SpatialVector_t>>::_Vector_impl" = type { %"struct.std::_Vector_base<SpatialVector_t, Eigen::aligned_allocator<SpatialVector_t>>::_Vector_impl_data" }
%"struct.std::_Vector_base<SpatialVector_t, Eigen::aligned_allocator<SpatialVector_t>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.21" = type { %"class.std::vector.22" }
%"class.std::vector.22" = type { %"struct.std::_Vector_base.23" }
%"struct.std::_Vector_base.23" = type { %"struct.std::_Vector_base<RigidBodyDynamics::Math::SpatialTransform, Eigen::aligned_allocator<RigidBodyDynamics::Math::SpatialTransform>>::_Vector_impl" }
%"struct.std::_Vector_base<RigidBodyDynamics::Math::SpatialTransform, Eigen::aligned_allocator<RigidBodyDynamics::Math::SpatialTransform>>::_Vector_impl" = type { %"struct.std::_Vector_base<RigidBodyDynamics::Math::SpatialTransform, Eigen::aligned_allocator<RigidBodyDynamics::Math::SpatialTransform>>::_Vector_impl_data" }
%"struct.std::_Vector_base<RigidBodyDynamics::Math::SpatialTransform, Eigen::aligned_allocator<RigidBodyDynamics::Math::SpatialTransform>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.154" = type { %"class.std::vector.155" }
%"class.std::vector.155" = type { %"struct.std::_Vector_base.156" }
%"struct.std::_Vector_base.156" = type { %"struct.std::_Vector_base<RigidBodyDynamics::FixedBody, Eigen::aligned_allocator<RigidBodyDynamics::FixedBody>>::_Vector_impl" }
%"struct.std::_Vector_base<RigidBodyDynamics::FixedBody, Eigen::aligned_allocator<RigidBodyDynamics::FixedBody>>::_Vector_impl" = type { %"struct.std::_Vector_base<RigidBodyDynamics::FixedBody, Eigen::aligned_allocator<RigidBodyDynamics::FixedBody>>::_Vector_impl_data" }
%"struct.std::_Vector_base<RigidBodyDynamics::FixedBody, Eigen::aligned_allocator<RigidBodyDynamics::FixedBody>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.161" = type { %"class.std::vector.162" }
%"class.std::vector.162" = type { %"struct.std::_Vector_base.163" }
%"struct.std::_Vector_base.163" = type { %"struct.std::_Vector_base<RigidBodyDynamics::Body, Eigen::aligned_allocator<RigidBodyDynamics::Body>>::_Vector_impl" }
%"struct.std::_Vector_base<RigidBodyDynamics::Body, Eigen::aligned_allocator<RigidBodyDynamics::Body>>::_Vector_impl" = type { %"struct.std::_Vector_base<RigidBodyDynamics::Body, Eigen::aligned_allocator<RigidBodyDynamics::Body>>::_Vector_impl_data" }
%"struct.std::_Vector_base<RigidBodyDynamics::Body, Eigen::aligned_allocator<RigidBodyDynamics::Body>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::map" = type { %"class.std::_Rb_tree" }
%"class.std::_Rb_tree" = type { %"struct.std::_Rb_tree<std::__cxx11::basic_string<char>, std::pair<const std::__cxx11::basic_string<char>, unsigned int>, std::_Select1st<std::pair<const std::__cxx11::basic_string<char>, unsigned int>>, std::less<std::__cxx11::basic_string<char>>>::_Rb_tree_impl" }
%"struct.std::_Rb_tree<std::__cxx11::basic_string<char>, std::pair<const std::__cxx11::basic_string<char>, unsigned int>, std::_Select1st<std::pair<const std::__cxx11::basic_string<char>, unsigned int>>, std::less<std::__cxx11::basic_string<char>>>::_Rb_tree_impl" = type { [8 x i8], %"struct.std::_Rb_tree_header" }
%"struct.std::_Rb_tree_header" = type { %"struct.std::_Rb_tree_node_base", i64 }
%"struct.std::_Rb_tree_node_base" = type { i32, ptr, ptr, ptr }
%"struct.RigidBodyDynamics::ConstraintSet" = type { i32, i8, %"class.std::vector", %"class.std::vector.1", %"class.std::vector.6", %"class.std::vector.6", %"class.std::vector.6", %"class.std::vector.11", %"class.std::vector.6", %"class.std::vector.16", %"class.std::vector.16", %"class.std::vector.6", %"class.std::vector.6", %"class.std::vector.21", %"class.std::vector.21", %"class.std::vector.27", %"class.std::vector.34", %"class.Eigen::Matrix", %"class.Eigen::Matrix", %"class.Eigen::Matrix", %"class.Eigen::Matrix", %"class.Eigen::Matrix", %"class.Eigen::Matrix", %"class.Eigen::Matrix.41", %"class.Eigen::Matrix", %"class.Eigen::Matrix", %"class.Eigen::Matrix.41", %"class.Eigen::Matrix.41", %"class.Eigen::Matrix", %"class.Eigen::Matrix", %"class.Eigen::Matrix.41", %"class.Eigen::Matrix.41", %"class.Eigen::Matrix.41", %"class.Eigen::Matrix.41", %"class.Eigen::HouseholderQR", %"class.Eigen::Matrix.41", %"class.Eigen::Matrix.41", %"class.Eigen::Matrix.41", %"class.Eigen::Matrix", %"class.Eigen::Matrix", %"class.Eigen::Matrix.41", %"class.Eigen::Matrix", %"class.Eigen::Matrix", %"class.Eigen::Matrix", %"class.std::vector.27", %"class.std::vector.27", %"class.std::vector.16", %"class.std::vector.27", %"class.std::vector.27", %"class.Eigen::Matrix", %"class.std::vector.60", %"class.std::vector.27", %"class.Eigen::Matrix", %"class.std::vector.16" }
%"class.std::vector" = type { %"struct.std::_Vector_base" }
%"struct.std::_Vector_base" = type { %"struct.std::_Vector_base<RigidBodyDynamics::ConstraintSet::ConstraintType, std::allocator<RigidBodyDynamics::ConstraintSet::ConstraintType>>::_Vector_impl" }
%"struct.std::_Vector_base<RigidBodyDynamics::ConstraintSet::ConstraintType, std::allocator<RigidBodyDynamics::ConstraintSet::ConstraintType>>::_Vector_impl" = type { %"struct.std::_Vector_base<RigidBodyDynamics::ConstraintSet::ConstraintType, std::allocator<RigidBodyDynamics::ConstraintSet::ConstraintType>>::_Vector_impl_data" }
%"struct.std::_Vector_base<RigidBodyDynamics::ConstraintSet::ConstraintType, std::allocator<RigidBodyDynamics::ConstraintSet::ConstraintType>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.1" = type { %"struct.std::_Vector_base.2" }
%"struct.std::_Vector_base.2" = type { %"struct.std::_Vector_base<std::__cxx11::basic_string<char>, std::allocator<std::__cxx11::basic_string<char>>>::_Vector_impl" }
%"struct.std::_Vector_base<std::__cxx11::basic_string<char>, std::allocator<std::__cxx11::basic_string<char>>>::_Vector_impl" = type { %"struct.std::_Vector_base<std::__cxx11::basic_string<char>, std::allocator<std::__cxx11::basic_string<char>>>::_Vector_impl_data" }
%"struct.std::_Vector_base<std::__cxx11::basic_string<char>, std::allocator<std::__cxx11::basic_string<char>>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.11" = type { %"struct.std::_Vector_base.12" }
%"struct.std::_Vector_base.12" = type { %"struct.std::_Vector_base<RigidBodyDynamics::CustomConstraint *, std::allocator<RigidBodyDynamics::CustomConstraint *>>::_Vector_impl" }
%"struct.std::_Vector_base<RigidBodyDynamics::CustomConstraint *, std::allocator<RigidBodyDynamics::CustomConstraint *>>::_Vector_impl" = type { %"struct.std::_Vector_base<RigidBodyDynamics::CustomConstraint *, std::allocator<RigidBodyDynamics::CustomConstraint *>>::_Vector_impl_data" }
%"struct.std::_Vector_base<RigidBodyDynamics::CustomConstraint *, std::allocator<RigidBodyDynamics::CustomConstraint *>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.34" = type { %"struct.std::_Vector_base.35" }
%"struct.std::_Vector_base.35" = type { %"struct.std::_Vector_base<Vector2_t, std::allocator<Vector2_t>>::_Vector_impl" }
%"struct.std::_Vector_base<Vector2_t, std::allocator<Vector2_t>>::_Vector_impl" = type { %"struct.std::_Vector_base<Vector2_t, std::allocator<Vector2_t>>::_Vector_impl_data" }
%"struct.std::_Vector_base<Vector2_t, std::allocator<Vector2_t>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.Eigen::HouseholderQR" = type <{ %"class.Eigen::Matrix.41", %"class.Eigen::Matrix", %"class.Eigen::Matrix.51", i8, [7 x i8] }>
%"class.Eigen::Matrix.51" = type { %"class.Eigen::PlainObjectBase.52" }
%"class.Eigen::PlainObjectBase.52" = type { %"class.Eigen::DenseStorage.59" }
%"class.Eigen::DenseStorage.59" = type { ptr, i64 }
%"struct.RigidBodyDynamics::Joint" = type { ptr, i32, i32, i32, i32 }

@.str.4 = external hidden unnamed_addr constant [112 x i8], align 1
@_ZSt4cerr = external global %"class.std::basic_ostream", align 8
@.str.11 = external hidden unnamed_addr constant [25 x i8], align 1
@.str.12 = external hidden unnamed_addr constant [6 x i8], align 1
@__PRETTY_FUNCTION__._ZN17RigidBodyDynamics13CalcAssemblyQERNS_5ModelEN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_RKS4_dj = external hidden unnamed_addr constant [144 x i8], align 1
@.str.13 = external hidden unnamed_addr constant [29 x i8], align 1
@.str.14 = external hidden unnamed_addr constant [31 x i8], align 1

; Function Attrs: noreturn nounwind
declare void @__assert_fail(ptr noundef, ptr noundef, i32 noundef, ptr noundef) #0

; Function Attrs: nocallback nofree nosync nounwind willreturn memory(argmem: readwrite)
declare void @llvm.lifetime.start.p0(i64 immarg, ptr nocapture) #1

; Function Attrs: mustprogress uwtable
declare dso_local noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160)) #2 align 2

declare i32 @__gxx_personality_v0(...)

; Function Attrs: nocallback nofree nosync nounwind willreturn memory(argmem: readwrite)
declare void @llvm.lifetime.end.p0(i64 immarg, ptr nocapture) #1

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1), i64 noundef) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8), ptr noundef) #3

; Function Attrs: mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8), ptr noundef) #2 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_(ptr noundef nonnull align 8 dereferenceable(8)) #3

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(24) ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE7setZeroEv(ptr noundef nonnull align 1 dereferenceable(1)) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1)) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE4ZeroEll(ptr dead_on_unwind noalias writable sret(%"class.Eigen::CwiseNullaryOp.177") align 8, i64 noundef, i64 noundef) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4ZeroEl(ptr dead_on_unwind noalias writable sret(%"class.Eigen::CwiseNullaryOp.182") align 8, i64 noundef) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(56) ptr @_ZN5Eigen5BlockINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEELin1ELin1ELb0EEaSIS2_EERS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(56), ptr noundef nonnull align 1 dereferenceable(1)) #3 align 2

; Function Attrs: inlinehint mustprogress nounwind uwtable
declare dso_local void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16)) unnamed_addr #4 align 2

; Function Attrs: inlinehint mustprogress nounwind uwtable
declare dso_local void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #4 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics28CalcConstraintsPositionErrorERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_b(ptr noundef nonnull align 8 dereferenceable(872), ptr noundef nonnull align 8 dereferenceable(16), ptr noundef nonnull align 8 dereferenceable(1160), ptr noundef nonnull align 8 dereferenceable(16), i1 noundef zeroext) #2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef i64 @_ZNK5Eigen9EigenBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4sizeEv(ptr noundef nonnull align 1 dereferenceable(1)) #3 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics23CalcConstraintsJacobianERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERNS3_IdLin1ELin1ELi0ELin1ELin1EEEb(ptr noundef nonnull align 8 dereferenceable(872), ptr noundef nonnull align 8 dereferenceable(16), ptr noundef nonnull align 8 dereferenceable(1160), ptr noundef nonnull align 8 dereferenceable(24), i1 noundef zeroext) #2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1), i64 noundef, i64 noundef) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEC2INS_14CwiseNullaryOpINS_8internal18scalar_constant_opIdEES1_EEEERKNS_9EigenBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(16), ptr noundef nonnull align 1 dereferenceable(1)) unnamed_addr #3 align 2

; Function Attrs: mustprogress uwtable
define dso_local noundef zeroext i1 @_ZN17RigidBodyDynamics13CalcAssemblyQERNS_5ModelEN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_RKS4_dj(ptr noundef nonnull align 8 dereferenceable(872) %0, ptr noundef %1, ptr noundef nonnull align 8 dereferenceable(1160) %2, ptr noundef nonnull align 8 dereferenceable(16) %3, ptr noundef nonnull align 8 dereferenceable(16) %4, double noundef %5, i32 noundef %6) #2 personality ptr @__gxx_personality_v0 {
  %8 = alloca i1, align 1
  %9 = alloca ptr, align 8
  %10 = alloca ptr, align 8
  %11 = alloca ptr, align 8
  %12 = alloca ptr, align 8
  %13 = alloca ptr, align 8
  %14 = alloca double, align 8
  %15 = alloca i32, align 4
  %16 = alloca %"class.Eigen::Matrix.41", align 8
  %17 = alloca i64, align 8
  %18 = alloca %"class.Eigen::Matrix.41", align 8
  %19 = alloca %"class.Eigen::CwiseNullaryOp.177", align 8
  %20 = alloca ptr, align 8
  %21 = alloca i32, align 4
  %22 = alloca %"class.Eigen::Matrix", align 8
  %23 = alloca %"class.Eigen::CwiseNullaryOp.182", align 8
  %24 = alloca %"class.Eigen::Matrix", align 8
  %25 = alloca %"class.Eigen::CwiseNullaryOp.182", align 8
  %26 = alloca %"class.Eigen::Matrix", align 8
  %27 = alloca %"class.Eigen::CwiseNullaryOp.182", align 8
  %28 = alloca %"class.Eigen::Matrix", align 8
  %29 = alloca %"class.Eigen::CwiseNullaryOp.182", align 8
  %30 = alloca i32, align 4
  %31 = alloca i32, align 4
  %32 = alloca i32, align 4
  %33 = alloca %"class.Eigen::Block", align 8
  %34 = alloca %"class.Eigen::Transpose", align 8
  %35 = alloca %"class.Eigen::Block", align 8
  %36 = alloca %"class.Eigen::CwiseUnaryOp", align 8
  %37 = alloca %"class.Eigen::Block.216", align 8
  %38 = alloca %"class.Eigen::Block.216", align 8
  %39 = alloca i64, align 8
  %40 = alloca %"class.RigidBodyDynamics::Math::Quaternion", align 16
  %41 = alloca %class.Vector3_t, align 8
  %42 = alloca %"class.Eigen::Block.496", align 8
  %43 = alloca %class.Vector4_t, align 16
  %44 = alloca double, align 8
  %45 = alloca i32, align 4
  %46 = alloca i64, align 8
  store ptr %0, ptr %9, align 8, !tbaa !5
  store ptr %1, ptr %10, align 8, !tbaa !5
  store ptr %2, ptr %11, align 8, !tbaa !5
  store ptr %3, ptr %12, align 8, !tbaa !5
  store ptr %4, ptr %13, align 8, !tbaa !5
  store double %5, ptr %14, align 8, !tbaa !9
  store i32 %6, ptr %15, align 4, !tbaa !11
  %47 = load ptr, ptr %12, align 8, !tbaa !5
  %48 = call noundef i64 @_ZNK5Eigen9EigenBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4sizeEv(ptr noundef nonnull align 1 dereferenceable(1) %47)
  %49 = load ptr, ptr %9, align 8, !tbaa !5
  %50 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %49, i32 0, i32 4
  %51 = load i32, ptr %50, align 4, !tbaa !13
  %52 = zext i32 %51 to i64
  %53 = icmp ne i64 %48, %52
  br i1 %53, label %54, label %57

54:                                               ; preds = %7
  %55 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cerr, ptr noundef @.str.11)
  %56 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %55, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
  call void @__assert_fail(ptr noundef @.str.12, ptr noundef @.str.4, i32 noundef 816, ptr noundef @__PRETTY_FUNCTION__._ZN17RigidBodyDynamics13CalcAssemblyQERNS_5ModelEN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_RKS4_dj) #6
  unreachable

57:                                               ; preds = %7
  %58 = call noundef i64 @_ZNK5Eigen9EigenBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4sizeEv(ptr noundef nonnull align 1 dereferenceable(1) %1)
  %59 = load ptr, ptr %9, align 8, !tbaa !5
  %60 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %59, i32 0, i32 4
  %61 = load i32, ptr %60, align 4, !tbaa !13
  %62 = zext i32 %61 to i64
  %63 = icmp ne i64 %58, %62
  br i1 %63, label %64, label %67

64:                                               ; preds = %57
  %65 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cerr, ptr noundef @.str.13)
  %66 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %65, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
  call void @__assert_fail(ptr noundef @.str.12, ptr noundef @.str.4, i32 noundef 821, ptr noundef @__PRETTY_FUNCTION__._ZN17RigidBodyDynamics13CalcAssemblyQERNS_5ModelEN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_RKS4_dj) #6
  unreachable

67:                                               ; preds = %57
  %68 = load ptr, ptr %13, align 8, !tbaa !5
  %69 = call noundef i64 @_ZNK5Eigen9EigenBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4sizeEv(ptr noundef nonnull align 1 dereferenceable(1) %68)
  %70 = load ptr, ptr %9, align 8, !tbaa !5
  %71 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %70, i32 0, i32 3
  %72 = load i32, ptr %71, align 8, !tbaa !92
  %73 = zext i32 %72 to i64
  %74 = icmp ne i64 %69, %73
  br i1 %74, label %75, label %78

75:                                               ; preds = %67
  %76 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cerr, ptr noundef @.str.14)
  %77 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %76, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
  call void @__assert_fail(ptr noundef @.str.12, ptr noundef @.str.4, i32 noundef 826, ptr noundef @__PRETTY_FUNCTION__._ZN17RigidBodyDynamics13CalcAssemblyQERNS_5ModelEN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_RKS4_dj) #6
  unreachable

78:                                               ; preds = %67
  call void @llvm.lifetime.start.p0(i64 24, ptr %16) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %17) #7
  %79 = load ptr, ptr %11, align 8, !tbaa !5
  %80 = call noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %79)
  store i64 %80, ptr %17, align 8, !tbaa !93
  %81 = load ptr, ptr %9, align 8, !tbaa !5
  %82 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %81, i32 0, i32 3
  call void @_ZN5Eigen6MatrixIdLin1ELin1ELi0ELin1ELin1EEC2ImjEERKT_RKT0_(ptr noundef nonnull align 8 dereferenceable(24) %16, ptr noundef nonnull align 8 dereferenceable(8) %17, ptr noundef nonnull align 4 dereferenceable(4) %82)
  call void @llvm.lifetime.end.p0(i64 8, ptr %17) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr %18) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr %19) #7
  %83 = load ptr, ptr %11, align 8, !tbaa !5
  %84 = invoke noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %83)
          to label %85 unwind label %140

85:                                               ; preds = %78
  %86 = load ptr, ptr %9, align 8, !tbaa !5
  %87 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %86, i32 0, i32 3
  %88 = load i32, ptr %87, align 8, !tbaa !92
  %89 = zext i32 %88 to i64
  %90 = add i64 %84, %89
  %91 = load ptr, ptr %11, align 8, !tbaa !5
  %92 = invoke noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %91)
          to label %93 unwind label %140

93:                                               ; preds = %85
  %94 = load ptr, ptr %9, align 8, !tbaa !5
  %95 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %94, i32 0, i32 3
  %96 = load i32, ptr %95, align 8, !tbaa !92
  %97 = zext i32 %96 to i64
  %98 = add i64 %92, %97
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE4ZeroEll(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseNullaryOp.177") align 8 %19, i64 noundef %90, i64 noundef %98)
          to label %99 unwind label %140

99:                                               ; preds = %93
  invoke void @_ZN5Eigen6MatrixIdLin1ELin1ELi0ELin1ELin1EEC2INS_14CwiseNullaryOpINS_8internal18scalar_constant_opIdEES1_EEEERKNS_9EigenBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %18, ptr noundef nonnull align 1 dereferenceable(1) %19)
          to label %100 unwind label %140

100:                                              ; preds = %99
  call void @llvm.lifetime.end.p0(i64 24, ptr %19) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr %22) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr %23) #7
  %101 = load ptr, ptr %11, align 8, !tbaa !5
  %102 = invoke noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %101)
          to label %103 unwind label %144

103:                                              ; preds = %100
  %104 = load ptr, ptr %9, align 8, !tbaa !5
  %105 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %104, i32 0, i32 3
  %106 = load i32, ptr %105, align 8, !tbaa !92
  %107 = zext i32 %106 to i64
  %108 = add i64 %102, %107
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4ZeroEl(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseNullaryOp.182") align 8 %23, i64 noundef %108)
          to label %109 unwind label %144

109:                                              ; preds = %103
  invoke void @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEC2INS_14CwiseNullaryOpINS_8internal18scalar_constant_opIdEES1_EEEERKNS_9EigenBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(16) %22, ptr noundef nonnull align 1 dereferenceable(1) %23)
          to label %110 unwind label %144

110:                                              ; preds = %109
  call void @llvm.lifetime.end.p0(i64 24, ptr %23) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr %24) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr %25) #7
  %111 = load ptr, ptr %11, align 8, !tbaa !5
  %112 = invoke noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %111)
          to label %113 unwind label %148

113:                                              ; preds = %110
  %114 = load ptr, ptr %9, align 8, !tbaa !5
  %115 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %114, i32 0, i32 3
  %116 = load i32, ptr %115, align 8, !tbaa !92
  %117 = zext i32 %116 to i64
  %118 = add i64 %112, %117
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4ZeroEl(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseNullaryOp.182") align 8 %25, i64 noundef %118)
          to label %119 unwind label %148

119:                                              ; preds = %113
  invoke void @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEC2INS_14CwiseNullaryOpINS_8internal18scalar_constant_opIdEES1_EEEERKNS_9EigenBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(16) %24, ptr noundef nonnull align 1 dereferenceable(1) %25)
          to label %120 unwind label %148

120:                                              ; preds = %119
  call void @llvm.lifetime.end.p0(i64 24, ptr %25) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr %26) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr %27) #7
  %121 = load ptr, ptr %9, align 8, !tbaa !5
  %122 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %121, i32 0, i32 3
  %123 = load i32, ptr %122, align 8, !tbaa !92
  %124 = zext i32 %123 to i64
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4ZeroEl(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseNullaryOp.182") align 8 %27, i64 noundef %124)
          to label %125 unwind label %152

125:                                              ; preds = %120
  invoke void @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEC2INS_14CwiseNullaryOpINS_8internal18scalar_constant_opIdEES1_EEEERKNS_9EigenBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(16) %26, ptr noundef nonnull align 1 dereferenceable(1) %27)
          to label %126 unwind label %152

126:                                              ; preds = %125
  call void @llvm.lifetime.end.p0(i64 24, ptr %27) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr %28) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr %29) #7
  %127 = load ptr, ptr %11, align 8, !tbaa !5
  %128 = invoke noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %127)
          to label %129 unwind label %156

129:                                              ; preds = %126
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4ZeroEl(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseNullaryOp.182") align 8 %29, i64 noundef %128)
          to label %130 unwind label %156

130:                                              ; preds = %129
  invoke void @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEC2INS_14CwiseNullaryOpINS_8internal18scalar_constant_opIdEES1_EEEERKNS_9EigenBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(16) %28, ptr noundef nonnull align 1 dereferenceable(1) %29)
          to label %131 unwind label %156

131:                                              ; preds = %130
  call void @llvm.lifetime.end.p0(i64 24, ptr %29) #7
  call void @llvm.lifetime.start.p0(i64 4, ptr %30) #7
  store i32 0, ptr %30, align 4, !tbaa !11
  br label %132

132:                                              ; preds = %177, %131
  %133 = load i32, ptr %30, align 4, !tbaa !11
  %134 = zext i32 %133 to i64
  %135 = load ptr, ptr %13, align 8, !tbaa !5
  %136 = invoke noundef i64 @_ZNK5Eigen9EigenBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4sizeEv(ptr noundef nonnull align 1 dereferenceable(1) %135)
          to label %137 unwind label %160

137:                                              ; preds = %132
  %138 = icmp slt i64 %134, %136
  br i1 %138, label %164, label %139

139:                                              ; preds = %137
  call void @llvm.lifetime.end.p0(i64 4, ptr %30) #7
  br label %180

140:                                              ; preds = %99, %93, %85, %78
  %141 = landingpad { ptr, i32 }
          cleanup
  %142 = extractvalue { ptr, i32 } %141, 0
  store ptr %142, ptr %20, align 8
  %143 = extractvalue { ptr, i32 } %141, 1
  store i32 %143, ptr %21, align 4
  call void @llvm.lifetime.end.p0(i64 24, ptr %19) #7
  br label %432

144:                                              ; preds = %109, %103, %100
  %145 = landingpad { ptr, i32 }
          cleanup
  %146 = extractvalue { ptr, i32 } %145, 0
  store ptr %146, ptr %20, align 8
  %147 = extractvalue { ptr, i32 } %145, 1
  store i32 %147, ptr %21, align 4
  call void @llvm.lifetime.end.p0(i64 24, ptr %23) #7
  br label %431

148:                                              ; preds = %119, %113, %110
  %149 = landingpad { ptr, i32 }
          cleanup
  %150 = extractvalue { ptr, i32 } %149, 0
  store ptr %150, ptr %20, align 8
  %151 = extractvalue { ptr, i32 } %149, 1
  store i32 %151, ptr %21, align 4
  call void @llvm.lifetime.end.p0(i64 24, ptr %25) #7
  br label %430

152:                                              ; preds = %125, %120
  %153 = landingpad { ptr, i32 }
          cleanup
  %154 = extractvalue { ptr, i32 } %153, 0
  store ptr %154, ptr %20, align 8
  %155 = extractvalue { ptr, i32 } %153, 1
  store i32 %155, ptr %21, align 4
  call void @llvm.lifetime.end.p0(i64 24, ptr %27) #7
  br label %429

156:                                              ; preds = %130, %129, %126
  %157 = landingpad { ptr, i32 }
          cleanup
  %158 = extractvalue { ptr, i32 } %157, 0
  store ptr %158, ptr %20, align 8
  %159 = extractvalue { ptr, i32 } %157, 1
  store i32 %159, ptr %21, align 4
  call void @llvm.lifetime.end.p0(i64 24, ptr %29) #7
  br label %428

160:                                              ; preds = %169, %164, %132
  %161 = landingpad { ptr, i32 }
          cleanup
  %162 = extractvalue { ptr, i32 } %161, 0
  store ptr %162, ptr %20, align 8
  %163 = extractvalue { ptr, i32 } %161, 1
  store i32 %163, ptr %21, align 4
  call void @llvm.lifetime.end.p0(i64 4, ptr %30) #7
  br label %427

164:                                              ; preds = %137
  %165 = load ptr, ptr %13, align 8, !tbaa !5
  %166 = load i32, ptr %30, align 4, !tbaa !11
  %167 = zext i32 %166 to i64
  %168 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %165, i64 noundef %167)
          to label %169 unwind label %160

169:                                              ; preds = %164
  %170 = load double, ptr %168, align 8, !tbaa !9
  %171 = load i32, ptr %30, align 4, !tbaa !11
  %172 = zext i32 %171 to i64
  %173 = load i32, ptr %30, align 4, !tbaa !11
  %174 = zext i32 %173 to i64
  %175 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %18, i64 noundef %172, i64 noundef %174)
          to label %176 unwind label %160

176:                                              ; preds = %169
  store double %170, ptr %175, align 8, !tbaa !9
  br label %177

177:                                              ; preds = %176
  %178 = load i32, ptr %30, align 4, !tbaa !11
  %179 = add i32 %178, 1
  store i32 %179, ptr %30, align 4, !tbaa !11
  br label %132, !llvm.loop !94

180:                                              ; preds = %139
  %181 = load ptr, ptr %9, align 8, !tbaa !5
  %182 = load ptr, ptr %11, align 8, !tbaa !5
  invoke void @_ZN17RigidBodyDynamics28CalcConstraintsPositionErrorERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_b(ptr noundef nonnull align 8 dereferenceable(872) %181, ptr noundef nonnull align 8 dereferenceable(16) %1, ptr noundef nonnull align 8 dereferenceable(1160) %182, ptr noundef nonnull align 8 dereferenceable(16) %28, i1 noundef zeroext true)
          to label %183 unwind label %192

183:                                              ; preds = %180
  %184 = invoke noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4normEv(ptr noundef nonnull align 1 dereferenceable(1) %28)
          to label %185 unwind label %192

185:                                              ; preds = %183
  %186 = load double, ptr %14, align 8, !tbaa !9
  %187 = fcmp olt double %184, %186
  br i1 %187, label %188, label %196

188:                                              ; preds = %185
  %189 = load ptr, ptr %12, align 8, !tbaa !5
  %190 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEaSERKS1_(ptr noundef nonnull align 8 dereferenceable(16) %189, ptr noundef nonnull align 8 dereferenceable(16) %1)
          to label %191 unwind label %192

191:                                              ; preds = %188
  store i1 true, ptr %8, align 1
  store i32 1, ptr %31, align 4
  br label %425

192:                                              ; preds = %421, %188, %183, %180
  %193 = landingpad { ptr, i32 }
          cleanup
  %194 = extractvalue { ptr, i32 } %193, 0
  store ptr %194, ptr %20, align 8
  %195 = extractvalue { ptr, i32 } %193, 1
  store i32 %195, ptr %21, align 4
  br label %427

196:                                              ; preds = %185
  call void @llvm.lifetime.start.p0(i64 4, ptr %32) #7
  store i32 0, ptr %32, align 4, !tbaa !11
  br label %197

197:                                              ; preds = %415, %196
  %198 = load i32, ptr %32, align 4, !tbaa !11
  %199 = load i32, ptr %15, align 4, !tbaa !11
  %200 = icmp ult i32 %198, %199
  br i1 %200, label %202, label %201

201:                                              ; preds = %197
  store i32 5, ptr %31, align 4
  br label %418

202:                                              ; preds = %197
  %203 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE7setZeroEv(ptr noundef nonnull align 1 dereferenceable(1) %16)
          to label %204 unwind label %265

204:                                              ; preds = %202
  %205 = load ptr, ptr %9, align 8, !tbaa !5
  %206 = load ptr, ptr %11, align 8, !tbaa !5
  invoke void @_ZN17RigidBodyDynamics23CalcConstraintsJacobianERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERNS3_IdLin1ELin1ELi0ELin1ELin1EEEb(ptr noundef nonnull align 8 dereferenceable(872) %205, ptr noundef nonnull align 8 dereferenceable(16) %1, ptr noundef nonnull align 8 dereferenceable(1160) %206, ptr noundef nonnull align 8 dereferenceable(24) %16, i1 noundef zeroext true)
          to label %207 unwind label %265

207:                                              ; preds = %204
  call void @llvm.lifetime.start.p0(i64 56, ptr %33) #7
  %208 = load ptr, ptr %9, align 8, !tbaa !5
  %209 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %208, i32 0, i32 3
  %210 = load i32, ptr %209, align 8, !tbaa !92
  %211 = zext i32 %210 to i64
  %212 = load ptr, ptr %11, align 8, !tbaa !5
  %213 = invoke noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %212)
          to label %214 unwind label %269

214:                                              ; preds = %207
  %215 = load ptr, ptr %9, align 8, !tbaa !5
  %216 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %215, i32 0, i32 3
  %217 = load i32, ptr %216, align 8, !tbaa !92
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE5blockImjEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind writable sret(%"class.Eigen::Block") align 8 %33, ptr noundef nonnull align 1 dereferenceable(1) %18, i64 noundef %211, i64 noundef 0, i64 noundef %213, i32 noundef %217)
          to label %218 unwind label %269

218:                                              ; preds = %214
  %219 = invoke noundef nonnull align 8 dereferenceable(56) ptr @_ZN5Eigen5BlockINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEELin1ELin1ELb0EEaSIS2_EERS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(56) %33, ptr noundef nonnull align 1 dereferenceable(1) %16)
          to label %220 unwind label %269

220:                                              ; preds = %218
  call void @llvm.lifetime.end.p0(i64 56, ptr %33) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %34) #7
  %221 = invoke ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1) %16)
          to label %222 unwind label %273

222:                                              ; preds = %220
  %223 = getelementptr inbounds %"class.Eigen::Transpose", ptr %34, i32 0, i32 0
  store ptr %221, ptr %223, align 8
  call void @llvm.lifetime.start.p0(i64 56, ptr %35) #7
  %224 = load ptr, ptr %9, align 8, !tbaa !5
  %225 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %224, i32 0, i32 3
  %226 = load i32, ptr %225, align 8, !tbaa !92
  %227 = zext i32 %226 to i64
  %228 = load ptr, ptr %9, align 8, !tbaa !5
  %229 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %228, i32 0, i32 3
  %230 = load i32, ptr %229, align 8, !tbaa !92
  %231 = load ptr, ptr %11, align 8, !tbaa !5
  %232 = invoke noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %231)
          to label %233 unwind label %277

233:                                              ; preds = %222
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE5blockIjmEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind writable sret(%"class.Eigen::Block") align 8 %35, ptr noundef nonnull align 1 dereferenceable(1) %18, i64 noundef 0, i64 noundef %227, i32 noundef %230, i64 noundef %232)
          to label %234 unwind label %277

234:                                              ; preds = %233
  %235 = invoke noundef nonnull align 8 dereferenceable(56) ptr @_ZN5Eigen5BlockINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEELin1ELin1ELb0EEaSINS_9TransposeIS2_EEEERS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(56) %35, ptr noundef nonnull align 1 dereferenceable(1) %34)
          to label %236 unwind label %277

236:                                              ; preds = %234
  call void @llvm.lifetime.end.p0(i64 56, ptr %35) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %34) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr %36) #7
  invoke void @_ZNK5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEEngEv(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseUnaryOp") align 8 %36, ptr noundef nonnull align 1 dereferenceable(1) %28)
          to label %237 unwind label %282

237:                                              ; preds = %236
  call void @llvm.lifetime.start.p0(i64 56, ptr %37) #7
  %238 = load ptr, ptr %9, align 8, !tbaa !5
  %239 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %238, i32 0, i32 3
  %240 = load i32, ptr %239, align 8, !tbaa !92
  %241 = zext i32 %240 to i64
  %242 = load ptr, ptr %11, align 8, !tbaa !5
  %243 = invoke noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %242)
          to label %244 unwind label %286

244:                                              ; preds = %237
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE5blockImiEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind writable sret(%"class.Eigen::Block.216") align 8 %37, ptr noundef nonnull align 1 dereferenceable(1) %22, i64 noundef %241, i64 noundef 0, i64 noundef %243, i32 noundef 1)
          to label %245 unwind label %286

245:                                              ; preds = %244
  %246 = invoke noundef nonnull align 8 dereferenceable(56) ptr @_ZN5Eigen5BlockINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELin1ELin1ELb0EEaSINS_12CwiseUnaryOpINS_8internal18scalar_opposite_opIdEEKS2_EEEERS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(56) %37, ptr noundef nonnull align 1 dereferenceable(1) %36)
          to label %247 unwind label %286

247:                                              ; preds = %245
  call void @llvm.lifetime.end.p0(i64 56, ptr %37) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr %36) #7
  %248 = load ptr, ptr %11, align 8, !tbaa !5
  %249 = getelementptr inbounds %"struct.RigidBodyDynamics::ConstraintSet", ptr %248, i32 0, i32 0
  %250 = load i32, ptr %249, align 8, !tbaa !96
  invoke void @_ZN17RigidBodyDynamics17SolveLinearSystemERKN5Eigen6MatrixIdLin1ELin1ELi0ELin1ELin1EEERKNS1_IdLin1ELi1ELi0ELin1ELi1EEERS5_NS_4Math12LinearSolverE(ptr noundef nonnull align 8 dereferenceable(24) %18, ptr noundef nonnull align 8 dereferenceable(16) %22, ptr noundef nonnull align 8 dereferenceable(16) %24, i32 noundef %250)
          to label %251 unwind label %265

251:                                              ; preds = %247
  call void @llvm.lifetime.start.p0(i64 56, ptr %38) #7
  %252 = load ptr, ptr %9, align 8, !tbaa !5
  %253 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %252, i32 0, i32 3
  %254 = load i32, ptr %253, align 8, !tbaa !92
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE5blockIjiEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind writable sret(%"class.Eigen::Block.216") align 8 %38, ptr noundef nonnull align 1 dereferenceable(1) %24, i64 noundef 0, i64 noundef 0, i32 noundef %254, i32 noundef 1)
          to label %255 unwind label %291

255:                                              ; preds = %251
  %256 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEaSINS_5BlockIS1_Lin1ELin1ELb0EEEEERS1_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(16) %26, ptr noundef nonnull align 1 dereferenceable(1) %38)
          to label %257 unwind label %291

257:                                              ; preds = %255
  call void @llvm.lifetime.end.p0(i64 56, ptr %38) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %39) #7
  store i64 0, ptr %39, align 8, !tbaa !93
  br label %258

258:                                              ; preds = %393, %257
  %259 = load i64, ptr %39, align 8, !tbaa !93
  %260 = load ptr, ptr %9, align 8, !tbaa !5
  %261 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %260, i32 0, i32 10
  %262 = call noundef i64 @_ZNKSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24) %261) #7
  %263 = icmp ult i64 %259, %262
  br i1 %263, label %295, label %264

264:                                              ; preds = %258
  store i32 8, ptr %31, align 4
  call void @llvm.lifetime.end.p0(i64 8, ptr %39) #7
  br label %397

265:                                              ; preds = %410, %405, %400, %397, %247, %204, %202
  %266 = landingpad { ptr, i32 }
          cleanup
  %267 = extractvalue { ptr, i32 } %266, 0
  store ptr %267, ptr %20, align 8
  %268 = extractvalue { ptr, i32 } %266, 1
  store i32 %268, ptr %21, align 4
  br label %420

269:                                              ; preds = %218, %214, %207
  %270 = landingpad { ptr, i32 }
          cleanup
  %271 = extractvalue { ptr, i32 } %270, 0
  store ptr %271, ptr %20, align 8
  %272 = extractvalue { ptr, i32 } %270, 1
  store i32 %272, ptr %21, align 4
  call void @llvm.lifetime.end.p0(i64 56, ptr %33) #7
  br label %420

273:                                              ; preds = %220
  %274 = landingpad { ptr, i32 }
          cleanup
  %275 = extractvalue { ptr, i32 } %274, 0
  store ptr %275, ptr %20, align 8
  %276 = extractvalue { ptr, i32 } %274, 1
  store i32 %276, ptr %21, align 4
  br label %281

277:                                              ; preds = %234, %233, %222
  %278 = landingpad { ptr, i32 }
          cleanup
  %279 = extractvalue { ptr, i32 } %278, 0
  store ptr %279, ptr %20, align 8
  %280 = extractvalue { ptr, i32 } %278, 1
  store i32 %280, ptr %21, align 4
  call void @llvm.lifetime.end.p0(i64 56, ptr %35) #7
  br label %281

281:                                              ; preds = %277, %273
  call void @llvm.lifetime.end.p0(i64 8, ptr %34) #7
  br label %420

282:                                              ; preds = %236
  %283 = landingpad { ptr, i32 }
          cleanup
  %284 = extractvalue { ptr, i32 } %283, 0
  store ptr %284, ptr %20, align 8
  %285 = extractvalue { ptr, i32 } %283, 1
  store i32 %285, ptr %21, align 4
  br label %290

286:                                              ; preds = %245, %244, %237
  %287 = landingpad { ptr, i32 }
          cleanup
  %288 = extractvalue { ptr, i32 } %287, 0
  store ptr %288, ptr %20, align 8
  %289 = extractvalue { ptr, i32 } %287, 1
  store i32 %289, ptr %21, align 4
  call void @llvm.lifetime.end.p0(i64 56, ptr %37) #7
  br label %290

290:                                              ; preds = %286, %282
  call void @llvm.lifetime.end.p0(i64 16, ptr %36) #7
  br label %420

291:                                              ; preds = %255, %251
  %292 = landingpad { ptr, i32 }
          cleanup
  %293 = extractvalue { ptr, i32 } %292, 0
  store ptr %293, ptr %20, align 8
  %294 = extractvalue { ptr, i32 } %292, 1
  store i32 %294, ptr %21, align 4
  call void @llvm.lifetime.end.p0(i64 56, ptr %38) #7
  br label %420

295:                                              ; preds = %258
  %296 = load ptr, ptr %9, align 8, !tbaa !5
  %297 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %296, i32 0, i32 10
  %298 = load i64, ptr %39, align 8, !tbaa !93
  %299 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %297, i64 noundef %298) #7
  %300 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %299, i32 0, i32 1
  %301 = load i32, ptr %300, align 8, !tbaa !123
  %302 = icmp eq i32 %301, 6
  br i1 %302, label %303, label %350

303:                                              ; preds = %295
  call void @llvm.lifetime.start.p0(i64 32, ptr %40) #7
  %304 = load ptr, ptr %9, align 8, !tbaa !5
  %305 = load i64, ptr %39, align 8, !tbaa !93
  %306 = trunc i64 %305 to i32
  invoke void @_ZNK17RigidBodyDynamics5Model13GetQuaternionEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr dead_on_unwind writable sret(%"class.RigidBodyDynamics::Math::Quaternion") align 16 %40, ptr noundef nonnull align 8 dereferenceable(872) %304, i32 noundef %306, ptr noundef nonnull align 8 dereferenceable(16) %1)
          to label %307 unwind label %328

307:                                              ; preds = %303
  call void @llvm.lifetime.start.p0(i64 24, ptr %41) #7
  call void @llvm.lifetime.start.p0(i64 48, ptr %42) #7
  %308 = load ptr, ptr %9, align 8, !tbaa !5
  %309 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %308, i32 0, i32 10
  %310 = load i64, ptr %39, align 8, !tbaa !93
  %311 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %309, i64 noundef %310) #7
  %312 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %311, i32 0, i32 3
  %313 = load i32, ptr %312, align 8, !tbaa !126
  %314 = zext i32 %313 to i64
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE5blockILi3ELi1EEENS3_13FixedBlockXprIXT_EXT0_EE4TypeEll(ptr dead_on_unwind writable sret(%"class.Eigen::Block.496") align 8 %42, ptr noundef nonnull align 1 dereferenceable(1) %26, i64 noundef %314, i64 noundef 0)
          to label %315 unwind label %332

315:                                              ; preds = %307
  invoke void @_ZN9Vector3_tC2IN5Eigen5BlockINS1_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi3ELi1ELb0EEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %41, ptr noundef nonnull align 1 dereferenceable(1) %42)
          to label %316 unwind label %332

316:                                              ; preds = %315
  call void @llvm.lifetime.end.p0(i64 48, ptr %42) #7
  call void @llvm.lifetime.start.p0(i64 32, ptr %43) #7
  invoke void @_ZNK17RigidBodyDynamics4Math10Quaternion11omegaToQDotERK9Vector3_t(ptr dead_on_unwind writable sret(%class.Vector4_t) align 16 %43, ptr noundef nonnull align 16 dereferenceable(32) %40, ptr noundef nonnull align 8 dereferenceable(24) %41)
          to label %317 unwind label %336

317:                                              ; preds = %316
  %318 = invoke noundef nonnull align 16 dereferenceable(32) ptr @_ZN5Eigen10MatrixBaseINS_6MatrixIdLi4ELi1ELi0ELi4ELi1EEEEpLIS2_EERS2_RKNS0_IT_EE(ptr noundef nonnull align 1 dereferenceable(1) %40, ptr noundef nonnull align 1 dereferenceable(1) %43)
          to label %319 unwind label %336

319:                                              ; preds = %317
  call void @llvm.lifetime.end.p0(i64 32, ptr %43) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %44) #7
  %320 = invoke noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi4ELi1ELi0ELi4ELi1EEEE4normEv(ptr noundef nonnull align 1 dereferenceable(1) %40)
          to label %321 unwind label %340

321:                                              ; preds = %319
  store double %320, ptr %44, align 8, !tbaa !9
  %322 = invoke noundef nonnull align 16 dereferenceable(32) ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi4ELi1ELi0ELi4ELi1EEEEdVERKd(ptr noundef nonnull align 1 dereferenceable(1) %40, ptr noundef nonnull align 8 dereferenceable(8) %44)
          to label %323 unwind label %340

323:                                              ; preds = %321
  call void @llvm.lifetime.end.p0(i64 8, ptr %44) #7
  %324 = load ptr, ptr %9, align 8, !tbaa !5
  %325 = load i64, ptr %39, align 8, !tbaa !93
  %326 = trunc i64 %325 to i32
  invoke void @_ZNK17RigidBodyDynamics5Model13SetQuaternionEjRKNS_4Math10QuaternionERN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr noundef nonnull align 8 dereferenceable(872) %324, i32 noundef %326, ptr noundef nonnull align 16 dereferenceable(32) %40, ptr noundef nonnull align 8 dereferenceable(16) %1)
          to label %327 unwind label %344

327:                                              ; preds = %323
  call void @llvm.lifetime.end.p0(i64 24, ptr %41) #7
  call void @llvm.lifetime.end.p0(i64 32, ptr %40) #7
  br label %392

328:                                              ; preds = %303
  %329 = landingpad { ptr, i32 }
          cleanup
  %330 = extractvalue { ptr, i32 } %329, 0
  store ptr %330, ptr %20, align 8
  %331 = extractvalue { ptr, i32 } %329, 1
  store i32 %331, ptr %21, align 4
  br label %349

332:                                              ; preds = %315, %307
  %333 = landingpad { ptr, i32 }
          cleanup
  %334 = extractvalue { ptr, i32 } %333, 0
  store ptr %334, ptr %20, align 8
  %335 = extractvalue { ptr, i32 } %333, 1
  store i32 %335, ptr %21, align 4
  call void @llvm.lifetime.end.p0(i64 48, ptr %42) #7
  br label %348

336:                                              ; preds = %317, %316
  %337 = landingpad { ptr, i32 }
          cleanup
  %338 = extractvalue { ptr, i32 } %337, 0
  store ptr %338, ptr %20, align 8
  %339 = extractvalue { ptr, i32 } %337, 1
  store i32 %339, ptr %21, align 4
  call void @llvm.lifetime.end.p0(i64 32, ptr %43) #7
  br label %348

340:                                              ; preds = %321, %319
  %341 = landingpad { ptr, i32 }
          cleanup
  %342 = extractvalue { ptr, i32 } %341, 0
  store ptr %342, ptr %20, align 8
  %343 = extractvalue { ptr, i32 } %341, 1
  store i32 %343, ptr %21, align 4
  call void @llvm.lifetime.end.p0(i64 8, ptr %44) #7
  br label %348

344:                                              ; preds = %323
  %345 = landingpad { ptr, i32 }
          cleanup
  %346 = extractvalue { ptr, i32 } %345, 0
  store ptr %346, ptr %20, align 8
  %347 = extractvalue { ptr, i32 } %345, 1
  store i32 %347, ptr %21, align 4
  br label %348

348:                                              ; preds = %344, %340, %336, %332
  call void @llvm.lifetime.end.p0(i64 24, ptr %41) #7
  br label %349

349:                                              ; preds = %348, %328
  call void @llvm.lifetime.end.p0(i64 32, ptr %40) #7
  br label %396

350:                                              ; preds = %295
  call void @llvm.lifetime.start.p0(i64 4, ptr %45) #7
  %351 = load ptr, ptr %9, align 8, !tbaa !5
  %352 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %351, i32 0, i32 10
  %353 = load i64, ptr %39, align 8, !tbaa !93
  %354 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %352, i64 noundef %353) #7
  %355 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %354, i32 0, i32 3
  %356 = load i32, ptr %355, align 8, !tbaa !126
  store i32 %356, ptr %45, align 4, !tbaa !11
  call void @llvm.lifetime.start.p0(i64 8, ptr %46) #7
  store i64 0, ptr %46, align 8, !tbaa !93
  br label %357

357:                                              ; preds = %384, %350
  %358 = load i64, ptr %46, align 8, !tbaa !93
  %359 = load ptr, ptr %9, align 8, !tbaa !5
  %360 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %359, i32 0, i32 10
  %361 = load i64, ptr %39, align 8, !tbaa !93
  %362 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %360, i64 noundef %361) #7
  %363 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %362, i32 0, i32 2
  %364 = load i32, ptr %363, align 4, !tbaa !127
  %365 = zext i32 %364 to i64
  %366 = icmp ult i64 %358, %365
  br i1 %366, label %368, label %367

367:                                              ; preds = %357
  store i32 11, ptr %31, align 4
  call void @llvm.lifetime.end.p0(i64 8, ptr %46) #7
  br label %391

368:                                              ; preds = %357
  %369 = load i32, ptr %45, align 4, !tbaa !11
  %370 = zext i32 %369 to i64
  %371 = load i64, ptr %46, align 8, !tbaa !93
  %372 = add i64 %370, %371
  %373 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %26, i64 noundef %372)
          to label %374 unwind label %387

374:                                              ; preds = %368
  %375 = load double, ptr %373, align 8, !tbaa !9
  %376 = load i32, ptr %45, align 4, !tbaa !11
  %377 = zext i32 %376 to i64
  %378 = load i64, ptr %46, align 8, !tbaa !93
  %379 = add i64 %377, %378
  %380 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1, i64 noundef %379)
          to label %381 unwind label %387

381:                                              ; preds = %374
  %382 = load double, ptr %380, align 8, !tbaa !9
  %383 = fadd double %382, %375
  store double %383, ptr %380, align 8, !tbaa !9
  br label %384

384:                                              ; preds = %381
  %385 = load i64, ptr %46, align 8, !tbaa !93
  %386 = add i64 %385, 1
  store i64 %386, ptr %46, align 8, !tbaa !93
  br label %357, !llvm.loop !128

387:                                              ; preds = %374, %368
  %388 = landingpad { ptr, i32 }
          cleanup
  %389 = extractvalue { ptr, i32 } %388, 0
  store ptr %389, ptr %20, align 8
  %390 = extractvalue { ptr, i32 } %388, 1
  store i32 %390, ptr %21, align 4
  call void @llvm.lifetime.end.p0(i64 8, ptr %46) #7
  call void @llvm.lifetime.end.p0(i64 4, ptr %45) #7
  br label %396

391:                                              ; preds = %367
  call void @llvm.lifetime.end.p0(i64 4, ptr %45) #7
  br label %392

392:                                              ; preds = %391, %327
  br label %393

393:                                              ; preds = %392
  %394 = load i64, ptr %39, align 8, !tbaa !93
  %395 = add i64 %394, 1
  store i64 %395, ptr %39, align 8, !tbaa !93
  br label %258, !llvm.loop !129

396:                                              ; preds = %387, %349
  call void @llvm.lifetime.end.p0(i64 8, ptr %39) #7
  br label %420

397:                                              ; preds = %264
  %398 = load ptr, ptr %9, align 8, !tbaa !5
  %399 = load ptr, ptr %11, align 8, !tbaa !5
  invoke void @_ZN17RigidBodyDynamics28CalcConstraintsPositionErrorERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_b(ptr noundef nonnull align 8 dereferenceable(872) %398, ptr noundef nonnull align 8 dereferenceable(16) %1, ptr noundef nonnull align 8 dereferenceable(1160) %399, ptr noundef nonnull align 8 dereferenceable(16) %28, i1 noundef zeroext true)
          to label %400 unwind label %265

400:                                              ; preds = %397
  %401 = invoke noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4normEv(ptr noundef nonnull align 1 dereferenceable(1) %28)
          to label %402 unwind label %265

402:                                              ; preds = %400
  %403 = load double, ptr %14, align 8, !tbaa !9
  %404 = fcmp olt double %401, %403
  br i1 %404, label %405, label %414

405:                                              ; preds = %402
  %406 = invoke noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4normEv(ptr noundef nonnull align 1 dereferenceable(1) %26)
          to label %407 unwind label %265

407:                                              ; preds = %405
  %408 = load double, ptr %14, align 8, !tbaa !9
  %409 = fcmp olt double %406, %408
  br i1 %409, label %410, label %414

410:                                              ; preds = %407
  %411 = load ptr, ptr %12, align 8, !tbaa !5
  %412 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEaSERKS1_(ptr noundef nonnull align 8 dereferenceable(16) %411, ptr noundef nonnull align 8 dereferenceable(16) %1)
          to label %413 unwind label %265

413:                                              ; preds = %410
  store i1 true, ptr %8, align 1
  store i32 1, ptr %31, align 4
  br label %418

414:                                              ; preds = %407, %402
  br label %415

415:                                              ; preds = %414
  %416 = load i32, ptr %32, align 4, !tbaa !11
  %417 = add i32 %416, 1
  store i32 %417, ptr %32, align 4, !tbaa !11
  br label %197, !llvm.loop !130

418:                                              ; preds = %413, %201
  call void @llvm.lifetime.end.p0(i64 4, ptr %32) #7
  %419 = load i32, ptr %31, align 4
  switch i32 %419, label %425 [
    i32 5, label %421
  ]

420:                                              ; preds = %396, %291, %290, %281, %269, %265
  call void @llvm.lifetime.end.p0(i64 4, ptr %32) #7
  br label %427

421:                                              ; preds = %418
  %422 = load ptr, ptr %12, align 8, !tbaa !5
  %423 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEaSERKS1_(ptr noundef nonnull align 8 dereferenceable(16) %422, ptr noundef nonnull align 8 dereferenceable(16) %1)
          to label %424 unwind label %192

424:                                              ; preds = %421
  store i1 false, ptr %8, align 1
  store i32 1, ptr %31, align 4
  br label %425

425:                                              ; preds = %424, %418, %191
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %28) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr %28) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %26) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr %26) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %24) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr %24) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %22) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr %22) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(24) %18) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %18) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(24) %16) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %16) #7
  %426 = load i1, ptr %8, align 1
  ret i1 %426

427:                                              ; preds = %420, %192, %160
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %28) #7
  br label %428

428:                                              ; preds = %427, %156
  call void @llvm.lifetime.end.p0(i64 16, ptr %28) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %26) #7
  br label %429

429:                                              ; preds = %428, %152
  call void @llvm.lifetime.end.p0(i64 16, ptr %26) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %24) #7
  br label %430

430:                                              ; preds = %429, %148
  call void @llvm.lifetime.end.p0(i64 16, ptr %24) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %22) #7
  br label %431

431:                                              ; preds = %430, %144
  call void @llvm.lifetime.end.p0(i64 16, ptr %22) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(24) %18) #7
  br label %432

432:                                              ; preds = %431, %140
  call void @llvm.lifetime.end.p0(i64 24, ptr %18) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(24) %16) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %16) #7
  br label %433

433:                                              ; preds = %432
  %434 = load ptr, ptr %20, align 8
  %435 = load i32, ptr %21, align 4
  %436 = insertvalue { ptr, i32 } poison, ptr %434, 0
  %437 = insertvalue { ptr, i32 } %436, i32 %435, 1
  resume { ptr, i32 } %437
}

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen6MatrixIdLin1ELin1ELi0ELin1ELin1EEC2ImjEERKT_RKT0_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 4 dereferenceable(4)) unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen6MatrixIdLin1ELin1ELi0ELin1ELin1EEC2INS_14CwiseNullaryOpINS_8internal18scalar_constant_opIdEES1_EEEERKNS_9EigenBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 1 dereferenceable(1)) unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1), i64 noundef) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4normEv(ptr noundef nonnull align 1 dereferenceable(1)) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(16) ptr @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEaSERKS1_(ptr noundef nonnull align 8 dereferenceable(16), ptr noundef nonnull align 8 dereferenceable(16)) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE5blockImjEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind noalias writable sret(%"class.Eigen::Block") align 8, ptr noundef nonnull align 1 dereferenceable(1), i64 noundef, i64 noundef, i64 noundef, i32 noundef) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE5blockIjmEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind noalias writable sret(%"class.Eigen::Block") align 8, ptr noundef nonnull align 1 dereferenceable(1), i64 noundef, i64 noundef, i32 noundef, i64 noundef) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(56) ptr @_ZN5Eigen5BlockINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEELin1ELin1ELb0EEaSINS_9TransposeIS2_EEEERS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(56), ptr noundef nonnull align 1 dereferenceable(1)) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZNK5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEEngEv(ptr dead_on_unwind noalias writable sret(%"class.Eigen::CwiseUnaryOp") align 8, ptr noundef nonnull align 1 dereferenceable(1)) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE5blockImiEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind noalias writable sret(%"class.Eigen::Block.216") align 8, ptr noundef nonnull align 1 dereferenceable(1), i64 noundef, i64 noundef, i64 noundef, i32 noundef) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(56) ptr @_ZN5Eigen5BlockINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELin1ELin1ELb0EEaSINS_12CwiseUnaryOpINS_8internal18scalar_opposite_opIdEEKS2_EEEERS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(56), ptr noundef nonnull align 1 dereferenceable(1)) #3 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics17SolveLinearSystemERKN5Eigen6MatrixIdLin1ELin1ELi0ELin1ELin1EEERKNS1_IdLin1ELi1ELi0ELin1ELi1EEERS5_NS_4Math12LinearSolverE(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(16), ptr noundef nonnull align 8 dereferenceable(16), i32 noundef) #2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE5blockIjiEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind noalias writable sret(%"class.Eigen::Block.216") align 8, ptr noundef nonnull align 1 dereferenceable(1), i64 noundef, i64 noundef, i32 noundef, i32 noundef) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(16) ptr @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEaSINS_5BlockIS1_Lin1ELin1ELb0EEEEERS1_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(16), ptr noundef nonnull align 1 dereferenceable(1)) #3 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef i64 @_ZNKSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24)) #5 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24), i64 noundef) #5 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNK17RigidBodyDynamics5Model13GetQuaternionEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr dead_on_unwind noalias writable sret(%"class.RigidBodyDynamics::Math::Quaternion") align 16, ptr noundef nonnull align 8 dereferenceable(872), i32 noundef, ptr noundef nonnull align 8 dereferenceable(16)) #2 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE5blockILi3ELi1EEENS3_13FixedBlockXprIXT_EXT0_EE4TypeEll(ptr dead_on_unwind noalias writable sret(%"class.Eigen::Block.496") align 8, ptr noundef nonnull align 1 dereferenceable(1), i64 noundef, i64 noundef) #3 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN9Vector3_tC2IN5Eigen5BlockINS1_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi3ELi1ELb0EEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 1 dereferenceable(1)) unnamed_addr #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNK17RigidBodyDynamics4Math10Quaternion11omegaToQDotERK9Vector3_t(ptr dead_on_unwind noalias writable sret(%class.Vector4_t) align 16, ptr noundef nonnull align 16 dereferenceable(32), ptr noundef nonnull align 8 dereferenceable(24)) #2 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 16 dereferenceable(32) ptr @_ZN5Eigen10MatrixBaseINS_6MatrixIdLi4ELi1ELi0ELi4ELi1EEEEpLIS2_EERS2_RKNS0_IT_EE(ptr noundef nonnull align 1 dereferenceable(1), ptr noundef nonnull align 1 dereferenceable(1)) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi4ELi1ELi0ELi4ELi1EEEE4normEv(ptr noundef nonnull align 1 dereferenceable(1)) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 16 dereferenceable(32) ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi4ELi1ELi0ELi4ELi1EEEEdVERKd(ptr noundef nonnull align 1 dereferenceable(1), ptr noundef nonnull align 8 dereferenceable(8)) #3 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNK17RigidBodyDynamics5Model13SetQuaternionEjRKNS_4Math10QuaternionERN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr noundef nonnull align 8 dereferenceable(872), i32 noundef, ptr noundef nonnull align 16 dereferenceable(32), ptr noundef nonnull align 8 dereferenceable(16)) #2 align 2

attributes #0 = { noreturn nounwind "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #1 = { nocallback nofree nosync nounwind willreturn memory(argmem: readwrite) }
attributes #2 = { mustprogress uwtable "min-legal-vector-width"="0" "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #3 = { inlinehint mustprogress uwtable "min-legal-vector-width"="0" "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #4 = { inlinehint mustprogress nounwind uwtable "min-legal-vector-width"="0" "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #5 = { mustprogress nounwind uwtable "min-legal-vector-width"="0" "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #6 = { noreturn nounwind }
attributes #7 = { nounwind }

!llvm.module.flags = !{!0, !1, !2, !3}
!llvm.ident = !{!4}

!0 = !{i32 1, !"wchar_size", i32 4}
!1 = !{i32 8, !"PIC Level", i32 2}
!2 = !{i32 7, !"PIE Level", i32 2}
!3 = !{i32 7, !"uwtable", i32 2}
!4 = !{!"clang version 19.1.0 (git@github.com:llvm/llvm-project.git a4bf6cd7cfb1a1421ba92bca9d017b49936c55e4)"}
!5 = !{!6, !6, i64 0}
!6 = !{!"any pointer", !7, i64 0}
!7 = !{!"omnipotent char", !8, i64 0}
!8 = !{!"Simple C++ TBAA"}
!9 = !{!10, !10, i64 0}
!10 = !{!"double", !7, i64 0}
!11 = !{!12, !12, i64 0}
!12 = !{!"int", !7, i64 0}
!13 = !{!14, !12, i64 76}
!14 = !{!"_ZTSN17RigidBodyDynamics5ModelE", !15, i64 0, !15, i64 24, !19, i64 48, !12, i64 72, !12, i64 76, !12, i64 80, !12, i64 84, !23, i64 88, !28, i64 112, !28, i64 136, !33, i64 160, !28, i64 184, !28, i64 208, !28, i64 232, !15, i64 256, !38, i64 280, !15, i64 304, !43, i64 328, !43, i64 352, !48, i64 376, !52, i64 400, !15, i64 424, !56, i64 448, !28, i64 472, !60, i64 496, !28, i64 520, !28, i64 544, !65, i64 568, !65, i64 584, !28, i64 600, !69, i64 624, !69, i64 648, !28, i64 672, !28, i64 696, !38, i64 720, !38, i64 744, !74, i64 768, !12, i64 792, !79, i64 800, !84, i64 824}
!15 = !{!"_ZTSSt6vectorIjSaIjEE", !16, i64 0}
!16 = !{!"_ZTSSt12_Vector_baseIjSaIjEE", !17, i64 0}
!17 = !{!"_ZTSNSt12_Vector_baseIjSaIjEE12_Vector_implE", !18, i64 0}
!18 = !{!"_ZTSNSt12_Vector_baseIjSaIjEE17_Vector_impl_dataE", !6, i64 0, !6, i64 8, !6, i64 16}
!19 = !{!"_ZTSSt6vectorIS_IjSaIjEESaIS1_EE", !20, i64 0}
!20 = !{!"_ZTSSt12_Vector_baseISt6vectorIjSaIjEESaIS2_EE", !21, i64 0}
!21 = !{!"_ZTSNSt12_Vector_baseISt6vectorIjSaIjEESaIS2_EE12_Vector_implE", !22, i64 0}
!22 = !{!"_ZTSNSt12_Vector_baseISt6vectorIjSaIjEESaIS2_EE17_Vector_impl_dataE", !6, i64 0, !6, i64 8, !6, i64 16}
!23 = !{!"_ZTS9Vector3_t", !24, i64 0}
!24 = !{!"_ZTSN5Eigen6MatrixIdLi3ELi1ELi0ELi3ELi1EEE", !25, i64 0}
!25 = !{!"_ZTSN5Eigen15PlainObjectBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEE", !26, i64 0}
!26 = !{!"_ZTSN5Eigen12DenseStorageIdLi3ELi3ELi1ELi0EEE", !27, i64 0}
!27 = !{!"_ZTSN5Eigen8internal11plain_arrayIdLi3ELi0ELi0EEE", !7, i64 0}
!28 = !{!"_ZTSSt6vectorI15SpatialVector_tSaIS0_EE", !29, i64 0}
!29 = !{!"_ZTSSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEE", !30, i64 0}
!30 = !{!"_ZTSSt12_Vector_baseI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEE", !31, i64 0}
!31 = !{!"_ZTSNSt12_Vector_baseI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEE12_Vector_implE", !32, i64 0}
!32 = !{!"_ZTSNSt12_Vector_baseI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEE17_Vector_impl_dataE", !6, i64 0, !6, i64 8, !6, i64 16}
!33 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics5JointESaIS1_EE", !34, i64 0}
!34 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEE", !35, i64 0}
!35 = !{!"_ZTSSt12_Vector_baseIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEE", !36, i64 0}
!36 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEE12_Vector_implE", !37, i64 0}
!37 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEE17_Vector_impl_dataE", !6, i64 0, !6, i64 8, !6, i64 16}
!38 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformESaIS2_EE", !39, i64 0}
!39 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEE", !40, i64 0}
!40 = !{!"_ZTSSt12_Vector_baseIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEE", !41, i64 0}
!41 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEE12_Vector_implE", !42, i64 0}
!42 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEE17_Vector_impl_dataE", !6, i64 0, !6, i64 8, !6, i64 16}
!43 = !{!"_ZTSSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEESaIS2_EE", !44, i64 0}
!44 = !{!"_ZTSSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEE", !45, i64 0}
!45 = !{!"_ZTSSt12_Vector_baseIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEE", !46, i64 0}
!46 = !{!"_ZTSNSt12_Vector_baseIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEE12_Vector_implE", !47, i64 0}
!47 = !{!"_ZTSNSt12_Vector_baseIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEE17_Vector_impl_dataE", !6, i64 0, !6, i64 8, !6, i64 16}
!48 = !{!"_ZTSSt6vectorI9Matrix3_tSaIS0_EE", !49, i64 0}
!49 = !{!"_ZTSSt12_Vector_baseI9Matrix3_tSaIS0_EE", !50, i64 0}
!50 = !{!"_ZTSNSt12_Vector_baseI9Matrix3_tSaIS0_EE12_Vector_implE", !51, i64 0}
!51 = !{!"_ZTSNSt12_Vector_baseI9Matrix3_tSaIS0_EE17_Vector_impl_dataE", !6, i64 0, !6, i64 8, !6, i64 16}
!52 = !{!"_ZTSSt6vectorI9Vector3_tSaIS0_EE", !53, i64 0}
!53 = !{!"_ZTSSt12_Vector_baseI9Vector3_tSaIS0_EE", !54, i64 0}
!54 = !{!"_ZTSNSt12_Vector_baseI9Vector3_tSaIS0_EE12_Vector_implE", !55, i64 0}
!55 = !{!"_ZTSNSt12_Vector_baseI9Vector3_tSaIS0_EE17_Vector_impl_dataE", !6, i64 0, !6, i64 8, !6, i64 16}
!56 = !{!"_ZTSSt6vectorIPN17RigidBodyDynamics11CustomJointESaIS2_EE", !57, i64 0}
!57 = !{!"_ZTSSt12_Vector_baseIPN17RigidBodyDynamics11CustomJointESaIS2_EE", !58, i64 0}
!58 = !{!"_ZTSNSt12_Vector_baseIPN17RigidBodyDynamics11CustomJointESaIS2_EE12_Vector_implE", !59, i64 0}
!59 = !{!"_ZTSNSt12_Vector_baseIPN17RigidBodyDynamics11CustomJointESaIS2_EE17_Vector_impl_dataE", !6, i64 0, !6, i64 8, !6, i64 16}
!60 = !{!"_ZTSSt6vectorI15SpatialMatrix_tSaIS0_EE", !61, i64 0}
!61 = !{!"_ZTSSt6vectorI15SpatialMatrix_tN5Eigen17aligned_allocatorIS0_EEE", !62, i64 0}
!62 = !{!"_ZTSSt12_Vector_baseI15SpatialMatrix_tN5Eigen17aligned_allocatorIS0_EEE", !63, i64 0}
!63 = !{!"_ZTSNSt12_Vector_baseI15SpatialMatrix_tN5Eigen17aligned_allocatorIS0_EEE12_Vector_implE", !64, i64 0}
!64 = !{!"_ZTSNSt12_Vector_baseI15SpatialMatrix_tN5Eigen17aligned_allocatorIS0_EEE17_Vector_impl_dataE", !6, i64 0, !6, i64 8, !6, i64 16}
!65 = !{!"_ZTSN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE", !66, i64 0}
!66 = !{!"_ZTSN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEEE", !67, i64 0}
!67 = !{!"_ZTSN5Eigen12DenseStorageIdLin1ELin1ELi1ELi0EEE", !6, i64 0, !68, i64 8}
!68 = !{!"long", !7, i64 0}
!69 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics4Math23SpatialRigidBodyInertiaESaIS2_EE", !70, i64 0}
!70 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics4Math23SpatialRigidBodyInertiaEN5Eigen17aligned_allocatorIS2_EEE", !71, i64 0}
!71 = !{!"_ZTSSt12_Vector_baseIN17RigidBodyDynamics4Math23SpatialRigidBodyInertiaEN5Eigen17aligned_allocatorIS2_EEE", !72, i64 0}
!72 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics4Math23SpatialRigidBodyInertiaEN5Eigen17aligned_allocatorIS2_EEE12_Vector_implE", !73, i64 0}
!73 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics4Math23SpatialRigidBodyInertiaEN5Eigen17aligned_allocatorIS2_EEE17_Vector_impl_dataE", !6, i64 0, !6, i64 8, !6, i64 16}
!74 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics9FixedBodyESaIS1_EE", !75, i64 0}
!75 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics9FixedBodyEN5Eigen17aligned_allocatorIS1_EEE", !76, i64 0}
!76 = !{!"_ZTSSt12_Vector_baseIN17RigidBodyDynamics9FixedBodyEN5Eigen17aligned_allocatorIS1_EEE", !77, i64 0}
!77 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics9FixedBodyEN5Eigen17aligned_allocatorIS1_EEE12_Vector_implE", !78, i64 0}
!78 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics9FixedBodyEN5Eigen17aligned_allocatorIS1_EEE17_Vector_impl_dataE", !6, i64 0, !6, i64 8, !6, i64 16}
!79 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics4BodyESaIS1_EE", !80, i64 0}
!80 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics4BodyEN5Eigen17aligned_allocatorIS1_EEE", !81, i64 0}
!81 = !{!"_ZTSSt12_Vector_baseIN17RigidBodyDynamics4BodyEN5Eigen17aligned_allocatorIS1_EEE", !82, i64 0}
!82 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics4BodyEN5Eigen17aligned_allocatorIS1_EEE12_Vector_implE", !83, i64 0}
!83 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics4BodyEN5Eigen17aligned_allocatorIS1_EEE17_Vector_impl_dataE", !6, i64 0, !6, i64 8, !6, i64 16}
!84 = !{!"_ZTSSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEEjSt4lessIS5_ESaISt4pairIKS5_jEEE", !85, i64 0}
!85 = !{!"_ZTSSt8_Rb_treeINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt4pairIKS5_jESt10_Select1stIS8_ESt4lessIS5_ESaIS8_EE", !86, i64 0}
!86 = !{!"_ZTSNSt8_Rb_treeINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt4pairIKS5_jESt10_Select1stIS8_ESt4lessIS5_ESaIS8_EE13_Rb_tree_implISC_Lb1EEE", !87, i64 0, !89, i64 8}
!87 = !{!"_ZTSSt20_Rb_tree_key_compareISt4lessINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEEEE", !88, i64 0}
!88 = !{!"_ZTSSt4lessINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEEE"}
!89 = !{!"_ZTSSt15_Rb_tree_header", !90, i64 0, !68, i64 32}
!90 = !{!"_ZTSSt18_Rb_tree_node_base", !91, i64 0, !6, i64 8, !6, i64 16, !6, i64 24}
!91 = !{!"_ZTSSt14_Rb_tree_color", !7, i64 0}
!92 = !{!14, !12, i64 72}
!93 = !{!68, !68, i64 0}
!94 = distinct !{!94, !95}
!95 = !{!"llvm.loop.mustprogress"}
!96 = !{!97, !98, i64 0}
!97 = !{!"_ZTSN17RigidBodyDynamics13ConstraintSetE", !98, i64 0, !99, i64 4, !100, i64 8, !104, i64 32, !15, i64 56, !15, i64 80, !15, i64 104, !108, i64 128, !15, i64 152, !52, i64 176, !52, i64 200, !15, i64 224, !15, i64 248, !38, i64 272, !38, i64 296, !28, i64 320, !112, i64 344, !65, i64 368, !65, i64 384, !65, i64 400, !65, i64 416, !65, i64 432, !65, i64 448, !116, i64 464, !65, i64 488, !65, i64 504, !116, i64 520, !116, i64 544, !65, i64 568, !65, i64 584, !116, i64 600, !116, i64 624, !116, i64 648, !116, i64 672, !119, i64 696, !116, i64 760, !116, i64 784, !116, i64 808, !65, i64 832, !65, i64 848, !116, i64 864, !65, i64 888, !65, i64 904, !65, i64 920, !28, i64 936, !28, i64 960, !52, i64 984, !28, i64 1008, !28, i64 1032, !65, i64 1056, !60, i64 1072, !28, i64 1096, !65, i64 1120, !52, i64 1136}
!98 = !{!"_ZTSN17RigidBodyDynamics4Math12LinearSolverE", !7, i64 0}
!99 = !{!"bool", !7, i64 0}
!100 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics13ConstraintSet14ConstraintTypeESaIS2_EE", !101, i64 0}
!101 = !{!"_ZTSSt12_Vector_baseIN17RigidBodyDynamics13ConstraintSet14ConstraintTypeESaIS2_EE", !102, i64 0}
!102 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics13ConstraintSet14ConstraintTypeESaIS2_EE12_Vector_implE", !103, i64 0}
!103 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics13ConstraintSet14ConstraintTypeESaIS2_EE17_Vector_impl_dataE", !6, i64 0, !6, i64 8, !6, i64 16}
!104 = !{!"_ZTSSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EE", !105, i64 0}
!105 = !{!"_ZTSSt12_Vector_baseINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EE", !106, i64 0}
!106 = !{!"_ZTSNSt12_Vector_baseINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EE12_Vector_implE", !107, i64 0}
!107 = !{!"_ZTSNSt12_Vector_baseINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EE17_Vector_impl_dataE", !6, i64 0, !6, i64 8, !6, i64 16}
!108 = !{!"_ZTSSt6vectorIPN17RigidBodyDynamics16CustomConstraintESaIS2_EE", !109, i64 0}
!109 = !{!"_ZTSSt12_Vector_baseIPN17RigidBodyDynamics16CustomConstraintESaIS2_EE", !110, i64 0}
!110 = !{!"_ZTSNSt12_Vector_baseIPN17RigidBodyDynamics16CustomConstraintESaIS2_EE12_Vector_implE", !111, i64 0}
!111 = !{!"_ZTSNSt12_Vector_baseIPN17RigidBodyDynamics16CustomConstraintESaIS2_EE17_Vector_impl_dataE", !6, i64 0, !6, i64 8, !6, i64 16}
!112 = !{!"_ZTSSt6vectorI9Vector2_tSaIS0_EE", !113, i64 0}
!113 = !{!"_ZTSSt12_Vector_baseI9Vector2_tSaIS0_EE", !114, i64 0}
!114 = !{!"_ZTSNSt12_Vector_baseI9Vector2_tSaIS0_EE12_Vector_implE", !115, i64 0}
!115 = !{!"_ZTSNSt12_Vector_baseI9Vector2_tSaIS0_EE17_Vector_impl_dataE", !6, i64 0, !6, i64 8, !6, i64 16}
!116 = !{!"_ZTSN5Eigen6MatrixIdLin1ELin1ELi0ELin1ELin1EEE", !117, i64 0}
!117 = !{!"_ZTSN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEEE", !118, i64 0}
!118 = !{!"_ZTSN5Eigen12DenseStorageIdLin1ELin1ELin1ELi0EEE", !6, i64 0, !68, i64 8, !68, i64 16}
!119 = !{!"_ZTSN5Eigen13HouseholderQRINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEEE", !116, i64 0, !65, i64 24, !120, i64 40, !99, i64 56}
!120 = !{!"_ZTSN5Eigen6MatrixIdLi1ELin1ELi1ELi1ELin1EEE", !121, i64 0}
!121 = !{!"_ZTSN5Eigen15PlainObjectBaseINS_6MatrixIdLi1ELin1ELi1ELi1ELin1EEEEE", !122, i64 0}
!122 = !{!"_ZTSN5Eigen12DenseStorageIdLin1ELi1ELin1ELi1EEE", !6, i64 0, !68, i64 8}
!123 = !{!124, !125, i64 8}
!124 = !{!"_ZTSN17RigidBodyDynamics5JointE", !6, i64 0, !125, i64 8, !12, i64 12, !12, i64 16, !12, i64 20}
!125 = !{!"_ZTSN17RigidBodyDynamics9JointTypeE", !7, i64 0}
!126 = !{!124, !12, i64 16}
!127 = !{!124, !12, i64 12}
!128 = distinct !{!128, !95}
!129 = distinct !{!129, !95}
!130 = distinct !{!130, !95}


; Optimized IR (opt -passes='default<O3>'):
target datalayout = "e-m:e-p270:32:32-p271:32:32-p272:64:64-i64:64-i128:128-f80:128-n8:16:32:64-S128"
target triple = "x86_64-unknown-linux-gnu"

%"class.std::basic_ostream" = type { ptr, %"class.std::basic_ios" }
%"class.std::basic_ios" = type { %"class.std::ios_base", ptr, i8, i8, ptr, ptr, ptr, ptr }
%"class.std::ios_base" = type { ptr, i64, i64, i32, i32, i32, ptr, %"struct.std::ios_base::_Words", [8 x %"struct.std::ios_base::_Words"], i32, ptr, %"class.std::locale" }
%"struct.std::ios_base::_Words" = type { ptr, i64 }
%"class.std::locale" = type { ptr }
%"class.Eigen::CwiseNullaryOp.177" = type { %"class.Eigen::internal::variable_if_dynamic", %"class.Eigen::internal::variable_if_dynamic", %"struct.Eigen::internal::scalar_constant_op" }
%"class.Eigen::internal::variable_if_dynamic" = type { i64 }
%"struct.Eigen::internal::scalar_constant_op" = type { double }
%"class.Eigen::CwiseNullaryOp.182" = type { %"class.Eigen::internal::variable_if_dynamic", [8 x i8], %"struct.Eigen::internal::scalar_constant_op" }
%"class.Eigen::Matrix.41" = type { %"class.Eigen::PlainObjectBase.42" }
%"class.Eigen::PlainObjectBase.42" = type { %"class.Eigen::DenseStorage.49" }
%"class.Eigen::DenseStorage.49" = type { ptr, i64, i64 }
%"class.Eigen::Matrix" = type { %"class.Eigen::PlainObjectBase" }
%"class.Eigen::PlainObjectBase" = type { %"class.Eigen::DenseStorage" }
%"class.Eigen::DenseStorage" = type { ptr, i64 }
%"class.Eigen::Block" = type { %"class.Eigen::BlockImpl" }
%"class.Eigen::BlockImpl" = type { %"class.Eigen::internal::BlockImpl_dense" }
%"class.Eigen::internal::BlockImpl_dense" = type { %"class.Eigen::MapBase", ptr, %"class.Eigen::internal::variable_if_dynamic", %"class.Eigen::internal::variable_if_dynamic", i64 }
%"class.Eigen::MapBase" = type { %"class.Eigen::MapBase.202" }
%"class.Eigen::MapBase.202" = type { ptr, %"class.Eigen::internal::variable_if_dynamic", %"class.Eigen::internal::variable_if_dynamic" }
%"class.Eigen::Transpose" = type { ptr }
%"class.Eigen::CwiseUnaryOp" = type <{ ptr, [8 x i8] }>
%"class.Eigen::Block.216" = type { %"class.Eigen::BlockImpl.217" }
%"class.Eigen::BlockImpl.217" = type { %"class.Eigen::internal::BlockImpl_dense.218" }
%"class.Eigen::internal::BlockImpl_dense.218" = type { %"class.Eigen::MapBase.219", ptr, %"class.Eigen::internal::variable_if_dynamic", %"class.Eigen::internal::variable_if_dynamic", i64 }
%"class.Eigen::MapBase.219" = type { %"class.Eigen::MapBase.220" }
%"class.Eigen::MapBase.220" = type { ptr, %"class.Eigen::internal::variable_if_dynamic", %"class.Eigen::internal::variable_if_dynamic" }
%"class.RigidBodyDynamics::Math::Quaternion" = type { %class.Vector4_t }
%class.Vector4_t = type { %"class.Eigen::Matrix.486" }
%"class.Eigen::Matrix.486" = type { %"class.Eigen::PlainObjectBase.487" }
%"class.Eigen::PlainObjectBase.487" = type { %"class.Eigen::DenseStorage.494" }
%"class.Eigen::DenseStorage.494" = type { %"struct.Eigen::internal::plain_array.495" }
%"struct.Eigen::internal::plain_array.495" = type { [4 x double] }
%class.Vector3_t = type { %"class.Eigen::Matrix.70" }
%"class.Eigen::Matrix.70" = type { %"class.Eigen::PlainObjectBase.71" }
%"class.Eigen::PlainObjectBase.71" = type { %"class.Eigen::DenseStorage.78" }
%"class.Eigen::DenseStorage.78" = type { %"struct.Eigen::internal::plain_array" }
%"struct.Eigen::internal::plain_array" = type { [3 x double] }
%"class.Eigen::Block.496" = type { %"class.Eigen::BlockImpl.497" }
%"class.Eigen::BlockImpl.497" = type { %"class.Eigen::internal::BlockImpl_dense.498" }
%"class.Eigen::internal::BlockImpl_dense.498" = type { %"class.Eigen::MapBase.base.508", ptr, %"class.Eigen::internal::variable_if_dynamic", [8 x i8], i64 }
%"class.Eigen::MapBase.base.508" = type { %"class.Eigen::MapBase.base.507" }
%"class.Eigen::MapBase.base.507" = type <{ ptr, [2 x i8] }>

@.str.4 = external hidden unnamed_addr constant [112 x i8], align 1
@_ZSt4cerr = external global %"class.std::basic_ostream", align 8
@.str.11 = external hidden unnamed_addr constant [25 x i8], align 1
@.str.12 = external hidden unnamed_addr constant [6 x i8], align 1
@__PRETTY_FUNCTION__._ZN17RigidBodyDynamics13CalcAssemblyQERNS_5ModelEN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_RKS4_dj = external hidden unnamed_addr constant [144 x i8], align 1
@.str.13 = external hidden unnamed_addr constant [29 x i8], align 1
@.str.14 = external hidden unnamed_addr constant [31 x i8], align 1

; Function Attrs: noreturn nounwind
declare void @__assert_fail(ptr noundef, ptr noundef, i32 noundef, ptr noundef) local_unnamed_addr #0

; Function Attrs: mustprogress nocallback nofree nosync nounwind willreturn memory(argmem: readwrite)
declare void @llvm.lifetime.start.p0(i64 immarg, ptr nocapture) #1

; Function Attrs: mustprogress uwtable
declare dso_local noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160)) local_unnamed_addr #2 align 2

declare i32 @__gxx_personality_v0(...)

; Function Attrs: mustprogress nocallback nofree nosync nounwind willreturn memory(argmem: readwrite)
declare void @llvm.lifetime.end.p0(i64 immarg, ptr nocapture) #1

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1), i64 noundef) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8), ptr noundef) local_unnamed_addr #3

; Function Attrs: mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8), ptr noundef) local_unnamed_addr #2 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_(ptr noundef nonnull align 8 dereferenceable(8)) #3

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(24) ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE7setZeroEv(ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE4ZeroEll(ptr dead_on_unwind noalias writable sret(%"class.Eigen::CwiseNullaryOp.177") align 8, i64 noundef, i64 noundef) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4ZeroEl(ptr dead_on_unwind noalias writable sret(%"class.Eigen::CwiseNullaryOp.182") align 8, i64 noundef) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(56) ptr @_ZN5Eigen5BlockINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEELin1ELin1ELb0EEaSIS2_EERS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(56), ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress nounwind uwtable
declare dso_local void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16)) unnamed_addr #4 align 2

; Function Attrs: inlinehint mustprogress nounwind uwtable
declare dso_local void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #4 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics28CalcConstraintsPositionErrorERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_b(ptr noundef nonnull align 8 dereferenceable(872), ptr noundef nonnull align 8 dereferenceable(16), ptr noundef nonnull align 8 dereferenceable(1160), ptr noundef nonnull align 8 dereferenceable(16), i1 noundef zeroext) local_unnamed_addr #2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef i64 @_ZNK5Eigen9EigenBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4sizeEv(ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #3 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics23CalcConstraintsJacobianERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERNS3_IdLin1ELin1ELi0ELin1ELin1EEEb(ptr noundef nonnull align 8 dereferenceable(872), ptr noundef nonnull align 8 dereferenceable(16), ptr noundef nonnull align 8 dereferenceable(1160), ptr noundef nonnull align 8 dereferenceable(24), i1 noundef zeroext) local_unnamed_addr #2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1), i64 noundef, i64 noundef) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEC2INS_14CwiseNullaryOpINS_8internal18scalar_constant_opIdEES1_EEEERKNS_9EigenBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(16), ptr noundef nonnull align 1 dereferenceable(1)) unnamed_addr #3 align 2

; Function Attrs: mustprogress uwtable
define dso_local noundef zeroext i1 @_ZN17RigidBodyDynamics13CalcAssemblyQERNS_5ModelEN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_RKS4_dj(ptr noundef nonnull align 8 dereferenceable(872) %0, ptr noundef %1, ptr noundef nonnull align 8 dereferenceable(1160) %2, ptr noundef nonnull align 8 dereferenceable(16) %3, ptr noundef nonnull align 8 dereferenceable(16) %4, double noundef %5, i32 noundef %6) local_unnamed_addr #2 personality ptr @__gxx_personality_v0 {
  %8 = alloca %"class.Eigen::Matrix.41", align 8
  %9 = alloca i64, align 8
  %10 = alloca %"class.Eigen::Matrix.41", align 8
  %11 = alloca %"class.Eigen::CwiseNullaryOp.177", align 8
  %12 = alloca %"class.Eigen::Matrix", align 8
  %13 = alloca %"class.Eigen::CwiseNullaryOp.182", align 8
  %14 = alloca %"class.Eigen::Matrix", align 8
  %15 = alloca %"class.Eigen::CwiseNullaryOp.182", align 8
  %16 = alloca %"class.Eigen::Matrix", align 8
  %17 = alloca %"class.Eigen::CwiseNullaryOp.182", align 8
  %18 = alloca %"class.Eigen::Matrix", align 8
  %19 = alloca %"class.Eigen::CwiseNullaryOp.182", align 8
  %20 = alloca %"class.Eigen::Block", align 8
  %21 = alloca %"class.Eigen::Transpose", align 8
  %22 = alloca %"class.Eigen::Block", align 8
  %23 = alloca %"class.Eigen::CwiseUnaryOp", align 8
  %24 = alloca %"class.Eigen::Block.216", align 8
  %25 = alloca %"class.Eigen::Block.216", align 8
  %26 = alloca %"class.RigidBodyDynamics::Math::Quaternion", align 16
  %27 = alloca %class.Vector3_t, align 8
  %28 = alloca %"class.Eigen::Block.496", align 8
  %29 = alloca %class.Vector4_t, align 16
  %30 = alloca double, align 8
  %31 = tail call noundef i64 @_ZNK5Eigen9EigenBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4sizeEv(ptr noundef nonnull align 1 dereferenceable(1) %3)
  %32 = getelementptr inbounds i8, ptr %0, i64 76
  %33 = load i32, ptr %32, align 4, !tbaa !5
  %34 = zext i32 %33 to i64
  %.not = icmp eq i64 %31, %34
  br i1 %.not, label %38, label %35

35:                                               ; preds = %7
  %36 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cerr, ptr noundef nonnull @.str.11)
  %37 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %36, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
  tail call void @__assert_fail(ptr noundef nonnull @.str.12, ptr noundef nonnull @.str.4, i32 noundef 816, ptr noundef nonnull @__PRETTY_FUNCTION__._ZN17RigidBodyDynamics13CalcAssemblyQERNS_5ModelEN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_RKS4_dj) #6
  unreachable

38:                                               ; preds = %7
  %39 = tail call noundef i64 @_ZNK5Eigen9EigenBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4sizeEv(ptr noundef nonnull align 1 dereferenceable(1) %1)
  %40 = load i32, ptr %32, align 4, !tbaa !5
  %41 = zext i32 %40 to i64
  %.not94 = icmp eq i64 %39, %41
  br i1 %.not94, label %45, label %42

42:                                               ; preds = %38
  %43 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cerr, ptr noundef nonnull @.str.13)
  %44 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %43, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
  tail call void @__assert_fail(ptr noundef nonnull @.str.12, ptr noundef nonnull @.str.4, i32 noundef 821, ptr noundef nonnull @__PRETTY_FUNCTION__._ZN17RigidBodyDynamics13CalcAssemblyQERNS_5ModelEN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_RKS4_dj) #6
  unreachable

45:                                               ; preds = %38
  %46 = tail call noundef i64 @_ZNK5Eigen9EigenBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4sizeEv(ptr noundef nonnull align 1 dereferenceable(1) %4)
  %47 = getelementptr inbounds i8, ptr %0, i64 72
  %48 = load i32, ptr %47, align 8, !tbaa !88
  %49 = zext i32 %48 to i64
  %.not95 = icmp eq i64 %46, %49
  br i1 %.not95, label %53, label %50

50:                                               ; preds = %45
  %51 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cerr, ptr noundef nonnull @.str.14)
  %52 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %51, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
  tail call void @__assert_fail(ptr noundef nonnull @.str.12, ptr noundef nonnull @.str.4, i32 noundef 826, ptr noundef nonnull @__PRETTY_FUNCTION__._ZN17RigidBodyDynamics13CalcAssemblyQERNS_5ModelEN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_RKS4_dj) #6
  unreachable

53:                                               ; preds = %45
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %8) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %9) #7
  %54 = tail call noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %2)
  store i64 %54, ptr %9, align 8, !tbaa !89
  call void @_ZN5Eigen6MatrixIdLin1ELin1ELi0ELin1ELin1EEC2ImjEERKT_RKT0_(ptr noundef nonnull align 8 dereferenceable(24) %8, ptr noundef nonnull align 8 dereferenceable(8) %9, ptr noundef nonnull align 4 dereferenceable(4) %47)
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %9) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %10) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %11) #7
  %55 = invoke noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %2)
          to label %56 unwind label %95

56:                                               ; preds = %53
  %57 = load i32, ptr %47, align 8, !tbaa !88
  %58 = invoke noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %2)
          to label %59 unwind label %95

59:                                               ; preds = %56
  %60 = zext i32 %57 to i64
  %61 = add i64 %55, %60
  %62 = load i32, ptr %47, align 8, !tbaa !88
  %63 = zext i32 %62 to i64
  %64 = add i64 %58, %63
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE4ZeroEll(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseNullaryOp.177") align 8 %11, i64 noundef %61, i64 noundef %64)
          to label %65 unwind label %95

65:                                               ; preds = %59
  invoke void @_ZN5Eigen6MatrixIdLin1ELin1ELi0ELin1ELin1EEC2INS_14CwiseNullaryOpINS_8internal18scalar_constant_opIdEES1_EEEERKNS_9EigenBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %10, ptr noundef nonnull align 1 dereferenceable(1) %11)
          to label %66 unwind label %95

66:                                               ; preds = %65
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %11) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %12) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %13) #7
  %67 = invoke noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %2)
          to label %68 unwind label %97

68:                                               ; preds = %66
  %69 = load i32, ptr %47, align 8, !tbaa !88
  %70 = zext i32 %69 to i64
  %71 = add i64 %67, %70
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4ZeroEl(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseNullaryOp.182") align 8 %13, i64 noundef %71)
          to label %72 unwind label %97

72:                                               ; preds = %68
  invoke void @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEC2INS_14CwiseNullaryOpINS_8internal18scalar_constant_opIdEES1_EEEERKNS_9EigenBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(16) %12, ptr noundef nonnull align 1 dereferenceable(1) %13)
          to label %73 unwind label %97

73:                                               ; preds = %72
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %13) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %14) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %15) #7
  %74 = invoke noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %2)
          to label %75 unwind label %99

75:                                               ; preds = %73
  %76 = load i32, ptr %47, align 8, !tbaa !88
  %77 = zext i32 %76 to i64
  %78 = add i64 %74, %77
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4ZeroEl(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseNullaryOp.182") align 8 %15, i64 noundef %78)
          to label %79 unwind label %99

79:                                               ; preds = %75
  invoke void @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEC2INS_14CwiseNullaryOpINS_8internal18scalar_constant_opIdEES1_EEEERKNS_9EigenBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(16) %14, ptr noundef nonnull align 1 dereferenceable(1) %15)
          to label %80 unwind label %99

80:                                               ; preds = %79
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %15) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %16) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %17) #7
  %81 = load i32, ptr %47, align 8, !tbaa !88
  %82 = zext i32 %81 to i64
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4ZeroEl(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseNullaryOp.182") align 8 %17, i64 noundef %82)
          to label %83 unwind label %101

83:                                               ; preds = %80
  invoke void @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEC2INS_14CwiseNullaryOpINS_8internal18scalar_constant_opIdEES1_EEEERKNS_9EigenBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(16) %16, ptr noundef nonnull align 1 dereferenceable(1) %17)
          to label %84 unwind label %101

84:                                               ; preds = %83
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %17) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %18) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %19) #7
  %85 = invoke noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %2)
          to label %86 unwind label %103

86:                                               ; preds = %84
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4ZeroEl(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseNullaryOp.182") align 8 %19, i64 noundef %85)
          to label %87 unwind label %103

87:                                               ; preds = %86
  invoke void @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEC2INS_14CwiseNullaryOpINS_8internal18scalar_constant_opIdEES1_EEEERKNS_9EigenBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(16) %18, ptr noundef nonnull align 1 dereferenceable(1) %19)
          to label %88 unwind label %103

88:                                               ; preds = %87
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %19) #7
  br label %89

89:                                               ; preds = %112, %88
  %.077 = phi i32 [ 0, %88 ], [ %113, %112 ]
  %90 = zext i32 %.077 to i64
  %91 = invoke noundef i64 @_ZNK5Eigen9EigenBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4sizeEv(ptr noundef nonnull align 1 dereferenceable(1) %4)
          to label %92 unwind label %105

92:                                               ; preds = %89
  %93 = icmp sgt i64 %91, %90
  br i1 %93, label %107, label %94

94:                                               ; preds = %92
  invoke void @_ZN17RigidBodyDynamics28CalcConstraintsPositionErrorERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_b(ptr noundef nonnull align 8 dereferenceable(872) %0, ptr noundef nonnull align 8 dereferenceable(16) %1, ptr noundef nonnull align 8 dereferenceable(1160) %2, ptr noundef nonnull align 8 dereferenceable(16) %18, i1 noundef zeroext true)
          to label %114 unwind label %121

95:                                               ; preds = %65, %59, %56, %53
  %96 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %11) #7
  br label %253

97:                                               ; preds = %72, %68, %66
  %98 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %13) #7
  br label %252

99:                                               ; preds = %79, %75, %73
  %100 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %15) #7
  br label %251

101:                                              ; preds = %83, %80
  %102 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %17) #7
  br label %250

103:                                              ; preds = %87, %86, %84
  %104 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %19) #7
  br label %249

105:                                              ; preds = %109, %107, %89
  %106 = landingpad { ptr, i32 }
          cleanup
  br label %248

107:                                              ; preds = %92
  %108 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %4, i64 noundef %90)
          to label %109 unwind label %105

109:                                              ; preds = %107
  %110 = load double, ptr %108, align 8, !tbaa !90
  %111 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %10, i64 noundef %90, i64 noundef %90)
          to label %112 unwind label %105

112:                                              ; preds = %109
  store double %110, ptr %111, align 8, !tbaa !90
  %113 = add i32 %.077, 1
  br label %89, !llvm.loop !92

114:                                              ; preds = %94
  %115 = invoke noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4normEv(ptr noundef nonnull align 1 dereferenceable(1) %18)
          to label %116 unwind label %121

116:                                              ; preds = %114
  %117 = fcmp olt double %115, %5
  br i1 %117, label %119, label %.preheader

.preheader:                                       ; preds = %116
  %.not99116.not = icmp eq i32 %6, 0
  br i1 %.not99116.not, label %.critedge, label %.lr.ph118

.lr.ph118:                                        ; preds = %.preheader
  %118 = getelementptr inbounds i8, ptr %0, i64 160
  br label %123

119:                                              ; preds = %116
  %120 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEaSERKS1_(ptr noundef nonnull align 8 dereferenceable(16) %3, ptr noundef nonnull align 8 dereferenceable(16) %1)
          to label %247 unwind label %121

121:                                              ; preds = %.critedge, %119, %114, %94
  %122 = landingpad { ptr, i32 }
          cleanup
  br label %248

123:                                              ; preds = %.lr.ph118, %244
  %.075117 = phi i32 [ 0, %.lr.ph118 ], [ %245, %244 ]
  %124 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE7setZeroEv(ptr noundef nonnull align 1 dereferenceable(1) %8)
          to label %125 unwind label %.loopexit112

125:                                              ; preds = %123
  invoke void @_ZN17RigidBodyDynamics23CalcConstraintsJacobianERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERNS3_IdLin1ELin1ELi0ELin1ELin1EEEb(ptr noundef nonnull align 8 dereferenceable(872) %0, ptr noundef nonnull align 8 dereferenceable(16) %1, ptr noundef nonnull align 8 dereferenceable(1160) %2, ptr noundef nonnull align 8 dereferenceable(24) %8, i1 noundef zeroext true)
          to label %126 unwind label %.loopexit112

126:                                              ; preds = %125
  call void @llvm.lifetime.start.p0(i64 56, ptr nonnull %20) #7
  %127 = load i32, ptr %47, align 8, !tbaa !88
  %128 = invoke noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %2)
          to label %129 unwind label %159

129:                                              ; preds = %126
  %130 = zext i32 %127 to i64
  %131 = load i32, ptr %47, align 8, !tbaa !88
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE5blockImjEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::Block") align 8 %20, ptr noundef nonnull align 1 dereferenceable(1) %10, i64 noundef %130, i64 noundef 0, i64 noundef %128, i32 noundef %131)
          to label %132 unwind label %159

132:                                              ; preds = %129
  %133 = invoke noundef nonnull align 8 dereferenceable(56) ptr @_ZN5Eigen5BlockINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEELin1ELin1ELb0EEaSIS2_EERS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(56) %20, ptr noundef nonnull align 1 dereferenceable(1) %8)
          to label %134 unwind label %159

134:                                              ; preds = %132
  call void @llvm.lifetime.end.p0(i64 56, ptr nonnull %20) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %21) #7
  %135 = invoke ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1) %8)
          to label %136 unwind label %161

136:                                              ; preds = %134
  store ptr %135, ptr %21, align 8
  call void @llvm.lifetime.start.p0(i64 56, ptr nonnull %22) #7
  %137 = load i32, ptr %47, align 8, !tbaa !88
  %138 = invoke noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %2)
          to label %139 unwind label %163

139:                                              ; preds = %136
  %140 = zext i32 %137 to i64
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE5blockIjmEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::Block") align 8 %22, ptr noundef nonnull align 1 dereferenceable(1) %10, i64 noundef 0, i64 noundef %140, i32 noundef %137, i64 noundef %138)
          to label %141 unwind label %163

141:                                              ; preds = %139
  %142 = invoke noundef nonnull align 8 dereferenceable(56) ptr @_ZN5Eigen5BlockINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEELin1ELin1ELb0EEaSINS_9TransposeIS2_EEEERS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(56) %22, ptr noundef nonnull align 1 dereferenceable(1) %21)
          to label %143 unwind label %163

143:                                              ; preds = %141
  call void @llvm.lifetime.end.p0(i64 56, ptr nonnull %22) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %21) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %23) #7
  invoke void @_ZNK5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEEngEv(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseUnaryOp") align 8 %23, ptr noundef nonnull align 1 dereferenceable(1) %18)
          to label %144 unwind label %166

144:                                              ; preds = %143
  call void @llvm.lifetime.start.p0(i64 56, ptr nonnull %24) #7
  %145 = load i32, ptr %47, align 8, !tbaa !88
  %146 = invoke noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %2)
          to label %147 unwind label %168

147:                                              ; preds = %144
  %148 = zext i32 %145 to i64
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE5blockImiEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::Block.216") align 8 %24, ptr noundef nonnull align 1 dereferenceable(1) %12, i64 noundef %148, i64 noundef 0, i64 noundef %146, i32 noundef 1)
          to label %149 unwind label %168

149:                                              ; preds = %147
  %150 = invoke noundef nonnull align 8 dereferenceable(56) ptr @_ZN5Eigen5BlockINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELin1ELin1ELb0EEaSINS_12CwiseUnaryOpINS_8internal18scalar_opposite_opIdEEKS2_EEEERS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(56) %24, ptr noundef nonnull align 1 dereferenceable(1) %23)
          to label %151 unwind label %168

151:                                              ; preds = %149
  call void @llvm.lifetime.end.p0(i64 56, ptr nonnull %24) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %23) #7
  %152 = load i32, ptr %2, align 8, !tbaa !94
  invoke void @_ZN17RigidBodyDynamics17SolveLinearSystemERKN5Eigen6MatrixIdLin1ELin1ELi0ELin1ELin1EEERKNS1_IdLin1ELi1ELi0ELin1ELi1EEERS5_NS_4Math12LinearSolverE(ptr noundef nonnull align 8 dereferenceable(24) %10, ptr noundef nonnull align 8 dereferenceable(16) %12, ptr noundef nonnull align 8 dereferenceable(16) %14, i32 noundef %152)
          to label %153 unwind label %.loopexit112

153:                                              ; preds = %151
  call void @llvm.lifetime.start.p0(i64 56, ptr nonnull %25) #7
  %154 = load i32, ptr %47, align 8, !tbaa !88
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE5blockIjiEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::Block.216") align 8 %25, ptr noundef nonnull align 1 dereferenceable(1) %14, i64 noundef 0, i64 noundef 0, i32 noundef %154, i32 noundef 1)
          to label %155 unwind label %171

155:                                              ; preds = %153
  %156 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEaSINS_5BlockIS1_Lin1ELin1ELb0EEEEERS1_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(16) %16, ptr noundef nonnull align 1 dereferenceable(1) %25)
          to label %157 unwind label %171

157:                                              ; preds = %155
  call void @llvm.lifetime.end.p0(i64 56, ptr nonnull %25) #7
  %158 = call noundef i64 @_ZNKSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24) %118) #7
  %.not119 = icmp eq i64 %158, 0
  br i1 %.not119, label %._crit_edge, label %.lr.ph115

._crit_edge:                                      ; preds = %.loopexit, %157
  invoke void @_ZN17RigidBodyDynamics28CalcConstraintsPositionErrorERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_b(ptr noundef nonnull align 8 dereferenceable(872) %0, ptr noundef nonnull align 8 dereferenceable(16) %1, ptr noundef nonnull align 8 dereferenceable(1160) %2, ptr noundef nonnull align 8 dereferenceable(16) %18, i1 noundef zeroext true)
          to label %234 unwind label %.loopexit112

.loopexit112:                                     ; preds = %123, %125, %151, %._crit_edge, %234, %238
  %lpad.loopexit = landingpad { ptr, i32 }
          cleanup
  br label %248

.loopexit.split-lp:                               ; preds = %242
  %lpad.loopexit.split-lp = landingpad { ptr, i32 }
          cleanup
  br label %248

159:                                              ; preds = %132, %129, %126
  %160 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 56, ptr nonnull %20) #7
  br label %248

161:                                              ; preds = %134
  %162 = landingpad { ptr, i32 }
          cleanup
  br label %165

163:                                              ; preds = %141, %139, %136
  %164 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 56, ptr nonnull %22) #7
  br label %165

165:                                              ; preds = %163, %161
  %.pn = phi { ptr, i32 } [ %164, %163 ], [ %162, %161 ]
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %21) #7
  br label %248

166:                                              ; preds = %143
  %167 = landingpad { ptr, i32 }
          cleanup
  br label %170

168:                                              ; preds = %149, %147, %144
  %169 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 56, ptr nonnull %24) #7
  br label %170

170:                                              ; preds = %168, %166
  %.pn97 = phi { ptr, i32 } [ %169, %168 ], [ %167, %166 ]
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %23) #7
  br label %248

171:                                              ; preds = %155, %153
  %172 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 56, ptr nonnull %25) #7
  br label %248

.lr.ph115:                                        ; preds = %157, %.loopexit
  %.074114 = phi i64 [ %231, %.loopexit ], [ 0, %157 ]
  %173 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %118, i64 noundef %.074114) #7
  %174 = getelementptr inbounds i8, ptr %173, i64 8
  %175 = load i32, ptr %174, align 8, !tbaa !121
  %176 = icmp eq i32 %175, 6
  br i1 %176, label %177, label %206

177:                                              ; preds = %.lr.ph115
  call void @llvm.lifetime.start.p0(i64 32, ptr nonnull %26) #7
  %178 = trunc i64 %.074114 to i32
  invoke void @_ZNK17RigidBodyDynamics5Model13GetQuaternionEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr dead_on_unwind nonnull writable sret(%"class.RigidBodyDynamics::Math::Quaternion") align 16 %26, ptr noundef nonnull align 8 dereferenceable(872) %0, i32 noundef %178, ptr noundef nonnull align 8 dereferenceable(16) %1)
          to label %179 unwind label %194

179:                                              ; preds = %177
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %27) #7
  call void @llvm.lifetime.start.p0(i64 48, ptr nonnull %28) #7
  %180 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %118, i64 noundef %.074114) #7
  %181 = getelementptr inbounds i8, ptr %180, i64 16
  %182 = load i32, ptr %181, align 8, !tbaa !124
  %183 = zext i32 %182 to i64
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE5blockILi3ELi1EEENS3_13FixedBlockXprIXT_EXT0_EE4TypeEll(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::Block.496") align 8 %28, ptr noundef nonnull align 1 dereferenceable(1) %16, i64 noundef %183, i64 noundef 0)
          to label %184 unwind label %196

184:                                              ; preds = %179
  invoke void @_ZN9Vector3_tC2IN5Eigen5BlockINS1_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi3ELi1ELb0EEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %27, ptr noundef nonnull align 1 dereferenceable(1) %28)
          to label %185 unwind label %196

185:                                              ; preds = %184
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %28) #7
  call void @llvm.lifetime.start.p0(i64 32, ptr nonnull %29) #7
  invoke void @_ZNK17RigidBodyDynamics4Math10Quaternion11omegaToQDotERK9Vector3_t(ptr dead_on_unwind nonnull writable sret(%class.Vector4_t) align 16 %29, ptr noundef nonnull align 16 dereferenceable(32) %26, ptr noundef nonnull align 8 dereferenceable(24) %27)
          to label %186 unwind label %198

186:                                              ; preds = %185
  %187 = invoke noundef nonnull align 16 dereferenceable(32) ptr @_ZN5Eigen10MatrixBaseINS_6MatrixIdLi4ELi1ELi0ELi4ELi1EEEEpLIS2_EERS2_RKNS0_IT_EE(ptr noundef nonnull align 1 dereferenceable(1) %26, ptr noundef nonnull align 1 dereferenceable(1) %29)
          to label %188 unwind label %198

188:                                              ; preds = %186
  call void @llvm.lifetime.end.p0(i64 32, ptr nonnull %29) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %30) #7
  %189 = invoke noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi4ELi1ELi0ELi4ELi1EEEE4normEv(ptr noundef nonnull align 1 dereferenceable(1) %26)
          to label %190 unwind label %200

190:                                              ; preds = %188
  store double %189, ptr %30, align 8, !tbaa !90
  %191 = invoke noundef nonnull align 16 dereferenceable(32) ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi4ELi1ELi0ELi4ELi1EEEEdVERKd(ptr noundef nonnull align 1 dereferenceable(1) %26, ptr noundef nonnull align 8 dereferenceable(8) %30)
          to label %192 unwind label %200

192:                                              ; preds = %190
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %30) #7
  invoke void @_ZNK17RigidBodyDynamics5Model13SetQuaternionEjRKNS_4Math10QuaternionERN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr noundef nonnull align 8 dereferenceable(872) %0, i32 noundef %178, ptr noundef nonnull align 16 dereferenceable(32) %26, ptr noundef nonnull align 8 dereferenceable(16) %1)
          to label %193 unwind label %202

193:                                              ; preds = %192
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %27) #7
  call void @llvm.lifetime.end.p0(i64 32, ptr nonnull %26) #7
  br label %.loopexit

194:                                              ; preds = %177
  %195 = landingpad { ptr, i32 }
          cleanup
  br label %205

196:                                              ; preds = %184, %179
  %197 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %28) #7
  br label %204

198:                                              ; preds = %186, %185
  %199 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 32, ptr nonnull %29) #7
  br label %204

200:                                              ; preds = %190, %188
  %201 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %30) #7
  br label %204

202:                                              ; preds = %192
  %203 = landingpad { ptr, i32 }
          cleanup
  br label %204

204:                                              ; preds = %202, %200, %198, %196
  %.pn100 = phi { ptr, i32 } [ %203, %202 ], [ %201, %200 ], [ %199, %198 ], [ %197, %196 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %27) #7
  br label %205

205:                                              ; preds = %204, %194
  %.pn100.pn = phi { ptr, i32 } [ %.pn100, %204 ], [ %195, %194 ]
  call void @llvm.lifetime.end.p0(i64 32, ptr nonnull %26) #7
  br label %248

206:                                              ; preds = %.lr.ph115
  %207 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %118, i64 noundef %.074114) #7
  %208 = getelementptr inbounds i8, ptr %207, i64 16
  %209 = load i32, ptr %208, align 8, !tbaa !124
  %210 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %118, i64 noundef %.074114) #7
  %211 = getelementptr inbounds i8, ptr %210, i64 12
  %212 = load i32, ptr %211, align 4, !tbaa !125
  %.not120 = icmp eq i32 %212, 0
  br i1 %.not120, label %.loopexit, label %.lr.ph

.lr.ph:                                           ; preds = %206
  %213 = zext i32 %209 to i64
  br label %214

214:                                              ; preds = %.lr.ph, %220
  %.0113 = phi i64 [ 0, %.lr.ph ], [ %223, %220 ]
  %215 = add nuw nsw i64 %.0113, %213
  %216 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %16, i64 noundef %215)
          to label %217 unwind label %229

217:                                              ; preds = %214
  %218 = load double, ptr %216, align 8, !tbaa !90
  %219 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1, i64 noundef %215)
          to label %220 unwind label %229

220:                                              ; preds = %217
  %221 = load double, ptr %219, align 8, !tbaa !90
  %222 = fadd double %218, %221
  store double %222, ptr %219, align 8, !tbaa !90
  %223 = add nuw nsw i64 %.0113, 1
  %224 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %118, i64 noundef %.074114) #7
  %225 = getelementptr inbounds i8, ptr %224, i64 12
  %226 = load i32, ptr %225, align 4, !tbaa !125
  %227 = zext i32 %226 to i64
  %228 = icmp ult i64 %223, %227
  br i1 %228, label %214, label %.loopexit, !llvm.loop !126

229:                                              ; preds = %217, %214
  %230 = landingpad { ptr, i32 }
          cleanup
  br label %248

.loopexit:                                        ; preds = %220, %206, %193
  %231 = add nuw i64 %.074114, 1
  %232 = call noundef i64 @_ZNKSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24) %118) #7
  %233 = icmp ult i64 %231, %232
  br i1 %233, label %.lr.ph115, label %._crit_edge, !llvm.loop !127

234:                                              ; preds = %._crit_edge
  %235 = invoke noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4normEv(ptr noundef nonnull align 1 dereferenceable(1) %18)
          to label %236 unwind label %.loopexit112

236:                                              ; preds = %234
  %237 = fcmp olt double %235, %5
  br i1 %237, label %238, label %244

238:                                              ; preds = %236
  %239 = invoke noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4normEv(ptr noundef nonnull align 1 dereferenceable(1) %16)
          to label %240 unwind label %.loopexit112

240:                                              ; preds = %238
  %241 = fcmp olt double %239, %5
  br i1 %241, label %242, label %244

242:                                              ; preds = %240
  %243 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEaSERKS1_(ptr noundef nonnull align 8 dereferenceable(16) %3, ptr noundef nonnull align 8 dereferenceable(16) %1)
          to label %247 unwind label %.loopexit.split-lp

244:                                              ; preds = %236, %240
  %245 = add nuw i32 %.075117, 1
  %exitcond.not = icmp eq i32 %245, %6
  br i1 %exitcond.not, label %.critedge, label %123, !llvm.loop !128

.critedge:                                        ; preds = %244, %.preheader
  %246 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEaSERKS1_(ptr noundef nonnull align 8 dereferenceable(16) %3, ptr noundef nonnull align 8 dereferenceable(16) %1)
          to label %247 unwind label %121

247:                                              ; preds = %242, %.critedge, %119
  %.1 = phi i1 [ true, %119 ], [ false, %.critedge ], [ true, %242 ]
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %18) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %18) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %16) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %16) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %14) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %14) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %12) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %12) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(24) %10) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %10) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(24) %8) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %8) #7
  ret i1 %.1

248:                                              ; preds = %.loopexit112, %.loopexit.split-lp, %159, %165, %170, %171, %229, %205, %121, %105
  %.pn105 = phi { ptr, i32 } [ %106, %105 ], [ %122, %121 ], [ %172, %171 ], [ %.pn97, %170 ], [ %.pn, %165 ], [ %160, %159 ], [ %.pn100.pn, %205 ], [ %230, %229 ], [ %lpad.loopexit, %.loopexit112 ], [ %lpad.loopexit.split-lp, %.loopexit.split-lp ]
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %18) #7
  br label %249

249:                                              ; preds = %248, %103
  %.pn105.pn = phi { ptr, i32 } [ %.pn105, %248 ], [ %104, %103 ]
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %18) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %16) #7
  br label %250

250:                                              ; preds = %249, %101
  %.pn105.pn.pn = phi { ptr, i32 } [ %.pn105.pn, %249 ], [ %102, %101 ]
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %16) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %14) #7
  br label %251

251:                                              ; preds = %250, %99
  %.pn105.pn.pn.pn = phi { ptr, i32 } [ %.pn105.pn.pn, %250 ], [ %100, %99 ]
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %14) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %12) #7
  br label %252

252:                                              ; preds = %251, %97
  %.pn105.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn105.pn.pn.pn, %251 ], [ %98, %97 ]
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %12) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(24) %10) #7
  br label %253

253:                                              ; preds = %252, %95
  %.pn105.pn.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn105.pn.pn.pn.pn, %252 ], [ %96, %95 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %10) #7
  call void @_ZN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEED2Ev(ptr noundef nonnull align 8 dereferenceable(24) %8) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %8) #7
  resume { ptr, i32 } %.pn105.pn.pn.pn.pn.pn
}

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen6MatrixIdLin1ELin1ELi0ELin1ELin1EEC2ImjEERKT_RKT0_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 4 dereferenceable(4)) unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen6MatrixIdLin1ELin1ELi0ELin1ELin1EEC2INS_14CwiseNullaryOpINS_8internal18scalar_constant_opIdEES1_EEEERKNS_9EigenBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 1 dereferenceable(1)) unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1), i64 noundef) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4normEv(ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(16) ptr @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEaSERKS1_(ptr noundef nonnull align 8 dereferenceable(16), ptr noundef nonnull align 8 dereferenceable(16)) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE5blockImjEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind noalias writable sret(%"class.Eigen::Block") align 8, ptr noundef nonnull align 1 dereferenceable(1), i64 noundef, i64 noundef, i64 noundef, i32 noundef) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEE5blockIjmEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind noalias writable sret(%"class.Eigen::Block") align 8, ptr noundef nonnull align 1 dereferenceable(1), i64 noundef, i64 noundef, i32 noundef, i64 noundef) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(56) ptr @_ZN5Eigen5BlockINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEELin1ELin1ELb0EEaSINS_9TransposeIS2_EEEERS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(56), ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZNK5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEEngEv(ptr dead_on_unwind noalias writable sret(%"class.Eigen::CwiseUnaryOp") align 8, ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE5blockImiEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind noalias writable sret(%"class.Eigen::Block.216") align 8, ptr noundef nonnull align 1 dereferenceable(1), i64 noundef, i64 noundef, i64 noundef, i32 noundef) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(56) ptr @_ZN5Eigen5BlockINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELin1ELin1ELb0EEaSINS_12CwiseUnaryOpINS_8internal18scalar_opposite_opIdEEKS2_EEEERS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(56), ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #3 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics17SolveLinearSystemERKN5Eigen6MatrixIdLin1ELin1ELi0ELin1ELin1EEERKNS1_IdLin1ELi1ELi0ELin1ELi1EEERS5_NS_4Math12LinearSolverE(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(16), ptr noundef nonnull align 8 dereferenceable(16), i32 noundef) local_unnamed_addr #2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE5blockIjiEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind noalias writable sret(%"class.Eigen::Block.216") align 8, ptr noundef nonnull align 1 dereferenceable(1), i64 noundef, i64 noundef, i32 noundef, i32 noundef) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(16) ptr @_ZN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEaSINS_5BlockIS1_Lin1ELin1ELb0EEEEERS1_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(16), ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #3 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef i64 @_ZNKSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24)) local_unnamed_addr #5 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24), i64 noundef) local_unnamed_addr #5 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNK17RigidBodyDynamics5Model13GetQuaternionEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr dead_on_unwind noalias writable sret(%"class.RigidBodyDynamics::Math::Quaternion") align 16, ptr noundef nonnull align 8 dereferenceable(872), i32 noundef, ptr noundef nonnull align 8 dereferenceable(16)) local_unnamed_addr #2 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE5blockILi3ELi1EEENS3_13FixedBlockXprIXT_EXT0_EE4TypeEll(ptr dead_on_unwind noalias writable sret(%"class.Eigen::Block.496") align 8, ptr noundef nonnull align 1 dereferenceable(1), i64 noundef, i64 noundef) local_unnamed_addr #3 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN9Vector3_tC2IN5Eigen5BlockINS1_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi3ELi1ELb0EEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 1 dereferenceable(1)) unnamed_addr #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNK17RigidBodyDynamics4Math10Quaternion11omegaToQDotERK9Vector3_t(ptr dead_on_unwind noalias writable sret(%class.Vector4_t) align 16, ptr noundef nonnull align 16 dereferenceable(32), ptr noundef nonnull align 8 dereferenceable(24)) local_unnamed_addr #2 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 16 dereferenceable(32) ptr @_ZN5Eigen10MatrixBaseINS_6MatrixIdLi4ELi1ELi0ELi4ELi1EEEEpLIS2_EERS2_RKNS0_IT_EE(ptr noundef nonnull align 1 dereferenceable(1), ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi4ELi1ELi0ELi4ELi1EEEE4normEv(ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 16 dereferenceable(32) ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi4ELi1ELi0ELi4ELi1EEEEdVERKd(ptr noundef nonnull align 1 dereferenceable(1), ptr noundef nonnull align 8 dereferenceable(8)) local_unnamed_addr #3 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNK17RigidBodyDynamics5Model13SetQuaternionEjRKNS_4Math10QuaternionERN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr noundef nonnull align 8 dereferenceable(872), i32 noundef, ptr noundef nonnull align 16 dereferenceable(32), ptr noundef nonnull align 8 dereferenceable(16)) local_unnamed_addr #2 align 2

attributes #0 = { noreturn nounwind "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #1 = { mustprogress nocallback nofree nosync nounwind willreturn memory(argmem: readwrite) }
attributes #2 = { mustprogress uwtable "min-legal-vector-width"="0" "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #3 = { inlinehint mustprogress uwtable "min-legal-vector-width"="0" "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #4 = { inlinehint mustprogress nounwind uwtable "min-legal-vector-width"="0" "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #5 = { mustprogress nounwind uwtable "min-legal-vector-width"="0" "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #6 = { noreturn nounwind }
attributes #7 = { nounwind }

!llvm.module.flags = !{!0, !1, !2, !3}
!llvm.ident = !{!4}

!0 = !{i32 1, !"wchar_size", i32 4}
!1 = !{i32 8, !"PIC Level", i32 2}
!2 = !{i32 7, !"PIE Level", i32 2}
!3 = !{i32 7, !"uwtable", i32 2}
!4 = !{!"clang version 19.1.0 (git@github.com:llvm/llvm-project.git a4bf6cd7cfb1a1421ba92bca9d017b49936c55e4)"}
!5 = !{!6, !18, i64 76}
!6 = !{!"_ZTSN17RigidBodyDynamics5ModelE", !7, i64 0, !7, i64 24, !14, i64 48, !18, i64 72, !18, i64 76, !18, i64 80, !18, i64 84, !19, i64 88, !24, i64 112, !24, i64 136, !29, i64 160, !24, i64 184, !24, i64 208, !24, i64 232, !7, i64 256, !34, i64 280, !7, i64 304, !39, i64 328, !39, i64 352, !44, i64 376, !48, i64 400, !7, i64 424, !52, i64 448, !24, i64 472, !56, i64 496, !24, i64 520, !24, i64 544, !61, i64 568, !61, i64 584, !24, i64 600, !65, i64 624, !65, i64 648, !24, i64 672, !24, i64 696, !34, i64 720, !34, i64 744, !70, i64 768, !18, i64 792, !75, i64 800, !80, i64 824}
!7 = !{!"_ZTSSt6vectorIjSaIjEE", !8, i64 0}
!8 = !{!"_ZTSSt12_Vector_baseIjSaIjEE", !9, i64 0}
!9 = !{!"_ZTSNSt12_Vector_baseIjSaIjEE12_Vector_implE", !10, i64 0}
!10 = !{!"_ZTSNSt12_Vector_baseIjSaIjEE17_Vector_impl_dataE", !11, i64 0, !11, i64 8, !11, i64 16}
!11 = !{!"any pointer", !12, i64 0}
!12 = !{!"omnipotent char", !13, i64 0}
!13 = !{!"Simple C++ TBAA"}
!14 = !{!"_ZTSSt6vectorIS_IjSaIjEESaIS1_EE", !15, i64 0}
!15 = !{!"_ZTSSt12_Vector_baseISt6vectorIjSaIjEESaIS2_EE", !16, i64 0}
!16 = !{!"_ZTSNSt12_Vector_baseISt6vectorIjSaIjEESaIS2_EE12_Vector_implE", !17, i64 0}
!17 = !{!"_ZTSNSt12_Vector_baseISt6vectorIjSaIjEESaIS2_EE17_Vector_impl_dataE", !11, i64 0, !11, i64 8, !11, i64 16}
!18 = !{!"int", !12, i64 0}
!19 = !{!"_ZTS9Vector3_t", !20, i64 0}
!20 = !{!"_ZTSN5Eigen6MatrixIdLi3ELi1ELi0ELi3ELi1EEE", !21, i64 0}
!21 = !{!"_ZTSN5Eigen15PlainObjectBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEE", !22, i64 0}
!22 = !{!"_ZTSN5Eigen12DenseStorageIdLi3ELi3ELi1ELi0EEE", !23, i64 0}
!23 = !{!"_ZTSN5Eigen8internal11plain_arrayIdLi3ELi0ELi0EEE", !12, i64 0}
!24 = !{!"_ZTSSt6vectorI15SpatialVector_tSaIS0_EE", !25, i64 0}
!25 = !{!"_ZTSSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEE", !26, i64 0}
!26 = !{!"_ZTSSt12_Vector_baseI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEE", !27, i64 0}
!27 = !{!"_ZTSNSt12_Vector_baseI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEE12_Vector_implE", !28, i64 0}
!28 = !{!"_ZTSNSt12_Vector_baseI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEE17_Vector_impl_dataE", !11, i64 0, !11, i64 8, !11, i64 16}
!29 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics5JointESaIS1_EE", !30, i64 0}
!30 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEE", !31, i64 0}
!31 = !{!"_ZTSSt12_Vector_baseIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEE", !32, i64 0}
!32 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEE12_Vector_implE", !33, i64 0}
!33 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEE17_Vector_impl_dataE", !11, i64 0, !11, i64 8, !11, i64 16}
!34 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformESaIS2_EE", !35, i64 0}
!35 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEE", !36, i64 0}
!36 = !{!"_ZTSSt12_Vector_baseIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEE", !37, i64 0}
!37 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEE12_Vector_implE", !38, i64 0}
!38 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEE17_Vector_impl_dataE", !11, i64 0, !11, i64 8, !11, i64 16}
!39 = !{!"_ZTSSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEESaIS2_EE", !40, i64 0}
!40 = !{!"_ZTSSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEE", !41, i64 0}
!41 = !{!"_ZTSSt12_Vector_baseIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEE", !42, i64 0}
!42 = !{!"_ZTSNSt12_Vector_baseIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEE12_Vector_implE", !43, i64 0}
!43 = !{!"_ZTSNSt12_Vector_baseIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEE17_Vector_impl_dataE", !11, i64 0, !11, i64 8, !11, i64 16}
!44 = !{!"_ZTSSt6vectorI9Matrix3_tSaIS0_EE", !45, i64 0}
!45 = !{!"_ZTSSt12_Vector_baseI9Matrix3_tSaIS0_EE", !46, i64 0}
!46 = !{!"_ZTSNSt12_Vector_baseI9Matrix3_tSaIS0_EE12_Vector_implE", !47, i64 0}
!47 = !{!"_ZTSNSt12_Vector_baseI9Matrix3_tSaIS0_EE17_Vector_impl_dataE", !11, i64 0, !11, i64 8, !11, i64 16}
!48 = !{!"_ZTSSt6vectorI9Vector3_tSaIS0_EE", !49, i64 0}
!49 = !{!"_ZTSSt12_Vector_baseI9Vector3_tSaIS0_EE", !50, i64 0}
!50 = !{!"_ZTSNSt12_Vector_baseI9Vector3_tSaIS0_EE12_Vector_implE", !51, i64 0}
!51 = !{!"_ZTSNSt12_Vector_baseI9Vector3_tSaIS0_EE17_Vector_impl_dataE", !11, i64 0, !11, i64 8, !11, i64 16}
!52 = !{!"_ZTSSt6vectorIPN17RigidBodyDynamics11CustomJointESaIS2_EE", !53, i64 0}
!53 = !{!"_ZTSSt12_Vector_baseIPN17RigidBodyDynamics11CustomJointESaIS2_EE", !54, i64 0}
!54 = !{!"_ZTSNSt12_Vector_baseIPN17RigidBodyDynamics11CustomJointESaIS2_EE12_Vector_implE", !55, i64 0}
!55 = !{!"_ZTSNSt12_Vector_baseIPN17RigidBodyDynamics11CustomJointESaIS2_EE17_Vector_impl_dataE", !11, i64 0, !11, i64 8, !11, i64 16}
!56 = !{!"_ZTSSt6vectorI15SpatialMatrix_tSaIS0_EE", !57, i64 0}
!57 = !{!"_ZTSSt6vectorI15SpatialMatrix_tN5Eigen17aligned_allocatorIS0_EEE", !58, i64 0}
!58 = !{!"_ZTSSt12_Vector_baseI15SpatialMatrix_tN5Eigen17aligned_allocatorIS0_EEE", !59, i64 0}
!59 = !{!"_ZTSNSt12_Vector_baseI15SpatialMatrix_tN5Eigen17aligned_allocatorIS0_EEE12_Vector_implE", !60, i64 0}
!60 = !{!"_ZTSNSt12_Vector_baseI15SpatialMatrix_tN5Eigen17aligned_allocatorIS0_EEE17_Vector_impl_dataE", !11, i64 0, !11, i64 8, !11, i64 16}
!61 = !{!"_ZTSN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE", !62, i64 0}
!62 = !{!"_ZTSN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEEE", !63, i64 0}
!63 = !{!"_ZTSN5Eigen12DenseStorageIdLin1ELin1ELi1ELi0EEE", !11, i64 0, !64, i64 8}
!64 = !{!"long", !12, i64 0}
!65 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics4Math23SpatialRigidBodyInertiaESaIS2_EE", !66, i64 0}
!66 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics4Math23SpatialRigidBodyInertiaEN5Eigen17aligned_allocatorIS2_EEE", !67, i64 0}
!67 = !{!"_ZTSSt12_Vector_baseIN17RigidBodyDynamics4Math23SpatialRigidBodyInertiaEN5Eigen17aligned_allocatorIS2_EEE", !68, i64 0}
!68 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics4Math23SpatialRigidBodyInertiaEN5Eigen17aligned_allocatorIS2_EEE12_Vector_implE", !69, i64 0}
!69 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics4Math23SpatialRigidBodyInertiaEN5Eigen17aligned_allocatorIS2_EEE17_Vector_impl_dataE", !11, i64 0, !11, i64 8, !11, i64 16}
!70 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics9FixedBodyESaIS1_EE", !71, i64 0}
!71 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics9FixedBodyEN5Eigen17aligned_allocatorIS1_EEE", !72, i64 0}
!72 = !{!"_ZTSSt12_Vector_baseIN17RigidBodyDynamics9FixedBodyEN5Eigen17aligned_allocatorIS1_EEE", !73, i64 0}
!73 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics9FixedBodyEN5Eigen17aligned_allocatorIS1_EEE12_Vector_implE", !74, i64 0}
!74 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics9FixedBodyEN5Eigen17aligned_allocatorIS1_EEE17_Vector_impl_dataE", !11, i64 0, !11, i64 8, !11, i64 16}
!75 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics4BodyESaIS1_EE", !76, i64 0}
!76 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics4BodyEN5Eigen17aligned_allocatorIS1_EEE", !77, i64 0}
!77 = !{!"_ZTSSt12_Vector_baseIN17RigidBodyDynamics4BodyEN5Eigen17aligned_allocatorIS1_EEE", !78, i64 0}
!78 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics4BodyEN5Eigen17aligned_allocatorIS1_EEE12_Vector_implE", !79, i64 0}
!79 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics4BodyEN5Eigen17aligned_allocatorIS1_EEE17_Vector_impl_dataE", !11, i64 0, !11, i64 8, !11, i64 16}
!80 = !{!"_ZTSSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEEjSt4lessIS5_ESaISt4pairIKS5_jEEE", !81, i64 0}
!81 = !{!"_ZTSSt8_Rb_treeINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt4pairIKS5_jESt10_Select1stIS8_ESt4lessIS5_ESaIS8_EE", !82, i64 0}
!82 = !{!"_ZTSNSt8_Rb_treeINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt4pairIKS5_jESt10_Select1stIS8_ESt4lessIS5_ESaIS8_EE13_Rb_tree_implISC_Lb1EEE", !83, i64 0, !85, i64 8}
!83 = !{!"_ZTSSt20_Rb_tree_key_compareISt4lessINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEEEE", !84, i64 0}
!84 = !{!"_ZTSSt4lessINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEEE"}
!85 = !{!"_ZTSSt15_Rb_tree_header", !86, i64 0, !64, i64 32}
!86 = !{!"_ZTSSt18_Rb_tree_node_base", !87, i64 0, !11, i64 8, !11, i64 16, !11, i64 24}
!87 = !{!"_ZTSSt14_Rb_tree_color", !12, i64 0}
!88 = !{!6, !18, i64 72}
!89 = !{!64, !64, i64 0}
!90 = !{!91, !91, i64 0}
!91 = !{!"double", !12, i64 0}
!92 = distinct !{!92, !93}
!93 = !{!"llvm.loop.mustprogress"}
!94 = !{!95, !96, i64 0}
!95 = !{!"_ZTSN17RigidBodyDynamics13ConstraintSetE", !96, i64 0, !97, i64 4, !98, i64 8, !102, i64 32, !7, i64 56, !7, i64 80, !7, i64 104, !106, i64 128, !7, i64 152, !48, i64 176, !48, i64 200, !7, i64 224, !7, i64 248, !34, i64 272, !34, i64 296, !24, i64 320, !110, i64 344, !61, i64 368, !61, i64 384, !61, i64 400, !61, i64 416, !61, i64 432, !61, i64 448, !114, i64 464, !61, i64 488, !61, i64 504, !114, i64 520, !114, i64 544, !61, i64 568, !61, i64 584, !114, i64 600, !114, i64 624, !114, i64 648, !114, i64 672, !117, i64 696, !114, i64 760, !114, i64 784, !114, i64 808, !61, i64 832, !61, i64 848, !114, i64 864, !61, i64 888, !61, i64 904, !61, i64 920, !24, i64 936, !24, i64 960, !48, i64 984, !24, i64 1008, !24, i64 1032, !61, i64 1056, !56, i64 1072, !24, i64 1096, !61, i64 1120, !48, i64 1136}
!96 = !{!"_ZTSN17RigidBodyDynamics4Math12LinearSolverE", !12, i64 0}
!97 = !{!"bool", !12, i64 0}
!98 = !{!"_ZTSSt6vectorIN17RigidBodyDynamics13ConstraintSet14ConstraintTypeESaIS2_EE", !99, i64 0}
!99 = !{!"_ZTSSt12_Vector_baseIN17RigidBodyDynamics13ConstraintSet14ConstraintTypeESaIS2_EE", !100, i64 0}
!100 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics13ConstraintSet14ConstraintTypeESaIS2_EE12_Vector_implE", !101, i64 0}
!101 = !{!"_ZTSNSt12_Vector_baseIN17RigidBodyDynamics13ConstraintSet14ConstraintTypeESaIS2_EE17_Vector_impl_dataE", !11, i64 0, !11, i64 8, !11, i64 16}
!102 = !{!"_ZTSSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EE", !103, i64 0}
!103 = !{!"_ZTSSt12_Vector_baseINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EE", !104, i64 0}
!104 = !{!"_ZTSNSt12_Vector_baseINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EE12_Vector_implE", !105, i64 0}
!105 = !{!"_ZTSNSt12_Vector_baseINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EE17_Vector_impl_dataE", !11, i64 0, !11, i64 8, !11, i64 16}
!106 = !{!"_ZTSSt6vectorIPN17RigidBodyDynamics16CustomConstraintESaIS2_EE", !107, i64 0}
!107 = !{!"_ZTSSt12_Vector_baseIPN17RigidBodyDynamics16CustomConstraintESaIS2_EE", !108, i64 0}
!108 = !{!"_ZTSNSt12_Vector_baseIPN17RigidBodyDynamics16CustomConstraintESaIS2_EE12_Vector_implE", !109, i64 0}
!109 = !{!"_ZTSNSt12_Vector_baseIPN17RigidBodyDynamics16CustomConstraintESaIS2_EE17_Vector_impl_dataE", !11, i64 0, !11, i64 8, !11, i64 16}
!110 = !{!"_ZTSSt6vectorI9Vector2_tSaIS0_EE", !111, i64 0}
!111 = !{!"_ZTSSt12_Vector_baseI9Vector2_tSaIS0_EE", !112, i64 0}
!112 = !{!"_ZTSNSt12_Vector_baseI9Vector2_tSaIS0_EE12_Vector_implE", !113, i64 0}
!113 = !{!"_ZTSNSt12_Vector_baseI9Vector2_tSaIS0_EE17_Vector_impl_dataE", !11, i64 0, !11, i64 8, !11, i64 16}
!114 = !{!"_ZTSN5Eigen6MatrixIdLin1ELin1ELi0ELin1ELin1EEE", !115, i64 0}
!115 = !{!"_ZTSN5Eigen15PlainObjectBaseINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEEE", !116, i64 0}
!116 = !{!"_ZTSN5Eigen12DenseStorageIdLin1ELin1ELin1ELi0EEE", !11, i64 0, !64, i64 8, !64, i64 16}
!117 = !{!"_ZTSN5Eigen13HouseholderQRINS_6MatrixIdLin1ELin1ELi0ELin1ELin1EEEEE", !114, i64 0, !61, i64 24, !118, i64 40, !97, i64 56}
!118 = !{!"_ZTSN5Eigen6MatrixIdLi1ELin1ELi1ELi1ELin1EEE", !119, i64 0}
!119 = !{!"_ZTSN5Eigen15PlainObjectBaseINS_6MatrixIdLi1ELin1ELi1ELi1ELin1EEEEE", !120, i64 0}
!120 = !{!"_ZTSN5Eigen12DenseStorageIdLin1ELi1ELin1ELi1EEE", !11, i64 0, !64, i64 8}
!121 = !{!122, !123, i64 8}
!122 = !{!"_ZTSN17RigidBodyDynamics5JointE", !11, i64 0, !123, i64 8, !18, i64 12, !18, i64 16, !18, i64 20}
!123 = !{!"_ZTSN17RigidBodyDynamics9JointTypeE", !12, i64 0}
!124 = !{!122, !18, i64 16}
!125 = !{!122, !18, i64 12}
!126 = distinct !{!126, !93}
!127 = distinct !{!127, !93}
!128 = distinct !{!128, !93}


; Effective O3 passes: EarlyCSEPass,GlobalOptPass,IPSCCPPass,IndVarSimplifyPass,InferFunctionAttrsPass,InstCombinePass,JumpThreadingPass,LICMPass,LoopRotatePass,LoopSimplifyPass,LoopUnrollPass,ReassociatePass,SROAPass,SimplifyCFGPass,TailCallElimPass