; 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.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.Matrix3_t = type { %"class.Eigen::Matrix.79" }
%"class.Eigen::Matrix.79" = type { %"class.Eigen::PlainObjectBase.80" }
%"class.Eigen::PlainObjectBase.80" = type { %"class.Eigen::DenseStorage.87" }
%"class.Eigen::DenseStorage.87" = type { %"struct.Eigen::internal::plain_array.88" }
%"struct.Eigen::internal::plain_array.88" = type { [9 x double] }
%class.SpatialVector_t = type { %"class.Eigen::Matrix.89" }
%"class.Eigen::Matrix.89" = type { %"class.Eigen::PlainObjectBase.90" }
%"class.Eigen::PlainObjectBase.90" = type { %"class.Eigen::DenseStorage.97" }
%"class.Eigen::DenseStorage.97" = type { %"struct.Eigen::internal::plain_array.98" }
%"struct.Eigen::internal::plain_array.98" = type { [6 x double] }
%"class.Eigen::Product.390" = type { %"class.Eigen::Transpose.397", ptr }
%"class.Eigen::Transpose.397" = type { ptr }
%"class.Eigen::Product.405" = type { %"class.Eigen::Transpose.397", %"class.Eigen::CwiseBinaryOp.412" }
%"class.Eigen::CwiseBinaryOp.412" = type <{ ptr, ptr, [8 x i8] }>
%"class.Eigen::Block.419" = type { %"class.Eigen::BlockImpl.420" }
%"class.Eigen::BlockImpl.420" = type { %"class.Eigen::internal::BlockImpl_dense.421" }
%"class.Eigen::internal::BlockImpl_dense.421" = type { %"class.Eigen::MapBase.base.430", ptr, %"class.Eigen::internal::variable_if_dynamic", [8 x i8], i64 }
%"class.Eigen::MapBase.base.430" = type { %"class.Eigen::MapBase.base" }
%"class.Eigen::MapBase.base" = type <{ ptr, [2 x i8] }>
%"class.Eigen::internal::variable_if_dynamic" = type { i64 }
%"class.Eigen::Product.431" = type { %"class.Eigen::Transpose.438", ptr }
%"class.Eigen::Transpose.438" = type { 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.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.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.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 }
%"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.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.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.Eigen::Matrix" = type { %"class.Eigen::PlainObjectBase" }
%"class.Eigen::PlainObjectBase" = type { %"class.Eigen::DenseStorage" }
%"class.Eigen::DenseStorage" = type { ptr, i64 }
%"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 }
%"struct.RigidBodyDynamics::Math::SpatialTransform" = type { %class.Matrix3_t, %class.Vector3_t }

@.str.4 = external hidden unnamed_addr constant [112 x i8], align 1
@.str.9 = external hidden unnamed_addr constant [24 x i8], align 1
@__PRETTY_FUNCTION__._ZN17RigidBodyDynamics28CalcConstraintsPositionErrorERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_b = external hidden unnamed_addr constant [127 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

; 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: mustprogress nounwind uwtable
declare dso_local noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24), i64 noundef) #4 align 2

; Function Attrs: mustprogress uwtable
define dso_local 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) %3, i1 noundef zeroext %4) #2 {
  %6 = alloca ptr, align 8
  %7 = alloca ptr, align 8
  %8 = alloca ptr, align 8
  %9 = alloca ptr, align 8
  %10 = alloca i8, align 1
  %11 = alloca i32, align 4
  %12 = alloca i32, align 4
  %13 = alloca i32, align 4
  %14 = alloca i32, align 4
  %15 = alloca %class.Vector3_t, align 8
  %16 = alloca %class.Vector3_t, align 8
  %17 = alloca %class.Matrix3_t, align 8
  %18 = alloca %class.Matrix3_t, align 8
  %19 = alloca %class.Matrix3_t, align 8
  %20 = alloca %class.SpatialVector_t, align 16
  %21 = alloca %"class.Eigen::Product.390", align 8
  %22 = alloca %"class.Eigen::Transpose.397", align 8
  %23 = alloca %class.Matrix3_t, align 8
  %24 = alloca %"class.Eigen::Product.390", align 8
  %25 = alloca %"class.Eigen::Transpose.397", align 8
  %26 = alloca %class.Matrix3_t, align 8
  %27 = alloca %"class.Eigen::Product.390", align 8
  %28 = alloca %"class.Eigen::Transpose.397", align 8
  %29 = alloca %class.Vector3_t, align 8
  %30 = alloca %class.Vector3_t, align 8
  %31 = alloca %"class.Eigen::Product.405", align 8
  %32 = alloca %"class.Eigen::Transpose.397", align 8
  %33 = alloca %"class.Eigen::CwiseBinaryOp.412", align 8
  %34 = alloca %"class.Eigen::Block.419", align 8
  %35 = alloca %"class.Eigen::Product.431", align 8
  %36 = alloca %"class.Eigen::Transpose.438", align 8
  %37 = alloca i32, align 4
  %38 = alloca i32, align 4
  store ptr %0, ptr %6, align 8, !tbaa !5
  store ptr %1, ptr %7, align 8, !tbaa !5
  store ptr %2, ptr %8, align 8, !tbaa !5
  store ptr %3, ptr %9, align 8, !tbaa !5
  %39 = zext i1 %4 to i8
  store i8 %39, ptr %10, align 1, !tbaa !9
  %40 = load ptr, ptr %9, align 8, !tbaa !5
  %41 = call noundef i64 @_ZNK5Eigen9EigenBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4sizeEv(ptr noundef nonnull align 1 dereferenceable(1) %40)
  %42 = load ptr, ptr %8, align 8, !tbaa !5
  %43 = call noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %42)
  %44 = icmp eq i64 %41, %43
  br i1 %44, label %45, label %46

45:                                               ; preds = %5
  br label %48

46:                                               ; preds = %5
  call void @__assert_fail(ptr noundef @.str.9, ptr noundef @.str.4, i32 noundef 490, ptr noundef @__PRETTY_FUNCTION__._ZN17RigidBodyDynamics28CalcConstraintsPositionErrorERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_b) #7
  unreachable

47:                                               ; No predecessors!
  br label %48

48:                                               ; preds = %47, %45
  %49 = load i8, ptr %10, align 1, !tbaa !9, !range !11, !noundef !12
  %50 = trunc i8 %49 to i1
  br i1 %50, label %51, label %54

51:                                               ; preds = %48
  %52 = load ptr, ptr %6, align 8, !tbaa !5
  %53 = load ptr, ptr %7, align 8, !tbaa !5
  call void @_ZN17RigidBodyDynamics22UpdateKinematicsCustomERNS_5ModelEPKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEES6_S6_(ptr noundef nonnull align 8 dereferenceable(872) %52, ptr noundef %53, ptr noundef null, ptr noundef null)
  br label %54

54:                                               ; preds = %51, %48
  call void @llvm.lifetime.start.p0(i64 4, ptr %11) #8
  store i32 0, ptr %11, align 4, !tbaa !13
  br label %55

55:                                               ; preds = %74, %54
  %56 = load i32, ptr %11, align 4, !tbaa !13
  %57 = zext i32 %56 to i64
  %58 = load ptr, ptr %8, align 8, !tbaa !5
  %59 = getelementptr inbounds %"struct.RigidBodyDynamics::ConstraintSet", ptr %58, i32 0, i32 4
  %60 = call noundef i64 @_ZNKSt6vectorIjSaIjEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24) %59) #8
  %61 = icmp ult i64 %57, %60
  br i1 %61, label %63, label %62

62:                                               ; preds = %55
  call void @llvm.lifetime.end.p0(i64 4, ptr %11) #8
  br label %77

63:                                               ; preds = %55
  call void @llvm.lifetime.start.p0(i64 4, ptr %12) #8
  %64 = load ptr, ptr %8, align 8, !tbaa !5
  %65 = getelementptr inbounds %"struct.RigidBodyDynamics::ConstraintSet", ptr %64, i32 0, i32 4
  %66 = load i32, ptr %11, align 4, !tbaa !13
  %67 = zext i32 %66 to i64
  %68 = call noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt6vectorIjSaIjEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %65, i64 noundef %67) #8
  %69 = load i32, ptr %68, align 4, !tbaa !13
  store i32 %69, ptr %12, align 4, !tbaa !13
  %70 = load ptr, ptr %9, align 8, !tbaa !5
  %71 = load i32, ptr %12, align 4, !tbaa !13
  %72 = zext i32 %71 to i64
  %73 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %70, i64 noundef %72)
  store double 0.000000e+00, ptr %73, align 8, !tbaa !15
  call void @llvm.lifetime.end.p0(i64 4, ptr %12) #8
  br label %74

74:                                               ; preds = %63
  %75 = load i32, ptr %11, align 4, !tbaa !13
  %76 = add i32 %75, 1
  store i32 %76, ptr %11, align 4, !tbaa !13
  br label %55, !llvm.loop !17

77:                                               ; preds = %62
  call void @llvm.lifetime.start.p0(i64 4, ptr %13) #8
  store i32 0, ptr %13, align 4, !tbaa !13
  br label %78

78:                                               ; preds = %216, %77
  %79 = load i32, ptr %13, align 4, !tbaa !13
  %80 = zext i32 %79 to i64
  %81 = load ptr, ptr %8, align 8, !tbaa !5
  %82 = getelementptr inbounds %"struct.RigidBodyDynamics::ConstraintSet", ptr %81, i32 0, i32 5
  %83 = call noundef i64 @_ZNKSt6vectorIjSaIjEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24) %82) #8
  %84 = icmp ult i64 %80, %83
  br i1 %84, label %86, label %85

85:                                               ; preds = %78
  call void @llvm.lifetime.end.p0(i64 4, ptr %13) #8
  br label %219

86:                                               ; preds = %78
  call void @llvm.lifetime.start.p0(i64 4, ptr %14) #8
  %87 = load ptr, ptr %8, align 8, !tbaa !5
  %88 = getelementptr inbounds %"struct.RigidBodyDynamics::ConstraintSet", ptr %87, i32 0, i32 5
  %89 = load i32, ptr %13, align 4, !tbaa !13
  %90 = zext i32 %89 to i64
  %91 = call noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt6vectorIjSaIjEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %88, i64 noundef %90) #8
  %92 = load i32, ptr %91, align 4, !tbaa !13
  store i32 %92, ptr %14, align 4, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 24, ptr %15) #8
  call void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %15)
  call void @llvm.lifetime.start.p0(i64 24, ptr %16) #8
  call void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %16)
  call void @llvm.lifetime.start.p0(i64 72, ptr %17) #8
  call void @_ZN9Matrix3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(72) %17)
  call void @llvm.lifetime.start.p0(i64 72, ptr %18) #8
  call void @_ZN9Matrix3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(72) %18)
  call void @llvm.lifetime.start.p0(i64 72, ptr %19) #8
  call void @_ZN9Matrix3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(72) %19)
  call void @llvm.lifetime.start.p0(i64 48, ptr %20) #8
  call void @_ZN15SpatialVector_tC2Ev(ptr noundef nonnull align 16 dereferenceable(48) %20)
  call void @llvm.lifetime.start.p0(i64 16, ptr %21) #8
  call void @llvm.lifetime.start.p0(i64 8, ptr %22) #8
  call void @llvm.lifetime.start.p0(i64 72, ptr %23) #8
  %93 = load ptr, ptr %6, align 8, !tbaa !5
  %94 = load ptr, ptr %7, align 8, !tbaa !5
  %95 = load ptr, ptr %8, align 8, !tbaa !5
  %96 = getelementptr inbounds %"struct.RigidBodyDynamics::ConstraintSet", ptr %95, i32 0, i32 11
  %97 = load i32, ptr %14, align 4, !tbaa !13
  %98 = zext i32 %97 to i64
  %99 = call noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt6vectorIjSaIjEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %96, i64 noundef %98) #8
  %100 = load i32, ptr %99, align 4, !tbaa !13
  call void @_ZN17RigidBodyDynamics24CalcBodyWorldOrientationERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEjb(ptr dead_on_unwind writable sret(%class.Matrix3_t) align 8 %23, ptr noundef nonnull align 8 dereferenceable(872) %93, ptr noundef nonnull align 8 dereferenceable(16) %94, i32 noundef %100, i1 noundef zeroext false)
  %101 = call ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1) %23)
  %102 = getelementptr inbounds %"class.Eigen::Transpose.397", ptr %22, i32 0, i32 0
  store ptr %101, ptr %102, align 8
  %103 = load ptr, ptr %8, align 8, !tbaa !5
  %104 = getelementptr inbounds %"struct.RigidBodyDynamics::ConstraintSet", ptr %103, i32 0, i32 13
  %105 = load i32, ptr %14, align 4, !tbaa !13
  %106 = zext i32 %105 to i64
  %107 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %104, i64 noundef %106) #8
  %108 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %107, i32 0, i32 0
  %109 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_9TransposeINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEmlIS3_EEKNS_7ProductIS4_T_Li0EEERKNS0_IS8_EE(ptr noundef nonnull align 1 dereferenceable(1) %22, ptr noundef nonnull align 1 dereferenceable(1) %108)
  %110 = getelementptr inbounds { ptr, ptr }, ptr %21, i32 0, i32 0
  %111 = extractvalue { ptr, ptr } %109, 0
  store ptr %111, ptr %110, align 8
  %112 = getelementptr inbounds { ptr, ptr }, ptr %21, i32 0, i32 1
  %113 = extractvalue { ptr, ptr } %109, 1
  store ptr %113, ptr %112, align 8
  %114 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSIN5Eigen7ProductINS1_9TransposeINS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEES5_Li0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(72) %17, ptr noundef nonnull align 1 dereferenceable(1) %21)
  call void @llvm.lifetime.end.p0(i64 72, ptr %23) #8
  call void @llvm.lifetime.end.p0(i64 8, ptr %22) #8
  call void @llvm.lifetime.end.p0(i64 16, ptr %21) #8
  call void @llvm.lifetime.start.p0(i64 16, ptr %24) #8
  call void @llvm.lifetime.start.p0(i64 8, ptr %25) #8
  call void @llvm.lifetime.start.p0(i64 72, ptr %26) #8
  %115 = load ptr, ptr %6, align 8, !tbaa !5
  %116 = load ptr, ptr %7, align 8, !tbaa !5
  %117 = load ptr, ptr %8, align 8, !tbaa !5
  %118 = getelementptr inbounds %"struct.RigidBodyDynamics::ConstraintSet", ptr %117, i32 0, i32 12
  %119 = load i32, ptr %14, align 4, !tbaa !13
  %120 = zext i32 %119 to i64
  %121 = call noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt6vectorIjSaIjEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %118, i64 noundef %120) #8
  %122 = load i32, ptr %121, align 4, !tbaa !13
  call void @_ZN17RigidBodyDynamics24CalcBodyWorldOrientationERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEjb(ptr dead_on_unwind writable sret(%class.Matrix3_t) align 8 %26, ptr noundef nonnull align 8 dereferenceable(872) %115, ptr noundef nonnull align 8 dereferenceable(16) %116, i32 noundef %122, i1 noundef zeroext false)
  %123 = call ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1) %26)
  %124 = getelementptr inbounds %"class.Eigen::Transpose.397", ptr %25, i32 0, i32 0
  store ptr %123, ptr %124, align 8
  %125 = load ptr, ptr %8, align 8, !tbaa !5
  %126 = getelementptr inbounds %"struct.RigidBodyDynamics::ConstraintSet", ptr %125, i32 0, i32 14
  %127 = load i32, ptr %14, align 4, !tbaa !13
  %128 = zext i32 %127 to i64
  %129 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %126, i64 noundef %128) #8
  %130 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %129, i32 0, i32 0
  %131 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_9TransposeINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEmlIS3_EEKNS_7ProductIS4_T_Li0EEERKNS0_IS8_EE(ptr noundef nonnull align 1 dereferenceable(1) %25, ptr noundef nonnull align 1 dereferenceable(1) %130)
  %132 = getelementptr inbounds { ptr, ptr }, ptr %24, i32 0, i32 0
  %133 = extractvalue { ptr, ptr } %131, 0
  store ptr %133, ptr %132, align 8
  %134 = getelementptr inbounds { ptr, ptr }, ptr %24, i32 0, i32 1
  %135 = extractvalue { ptr, ptr } %131, 1
  store ptr %135, ptr %134, align 8
  %136 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSIN5Eigen7ProductINS1_9TransposeINS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEES5_Li0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(72) %18, ptr noundef nonnull align 1 dereferenceable(1) %24)
  call void @llvm.lifetime.end.p0(i64 72, ptr %26) #8
  call void @llvm.lifetime.end.p0(i64 8, ptr %25) #8
  call void @llvm.lifetime.end.p0(i64 16, ptr %24) #8
  call void @llvm.lifetime.start.p0(i64 16, ptr %27) #8
  call void @llvm.lifetime.start.p0(i64 8, ptr %28) #8
  %137 = call ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1) %17)
  %138 = getelementptr inbounds %"class.Eigen::Transpose.397", ptr %28, i32 0, i32 0
  store ptr %137, ptr %138, align 8
  %139 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_9TransposeINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEmlIS3_EEKNS_7ProductIS4_T_Li0EEERKNS0_IS8_EE(ptr noundef nonnull align 1 dereferenceable(1) %28, ptr noundef nonnull align 1 dereferenceable(1) %18)
  %140 = getelementptr inbounds { ptr, ptr }, ptr %27, i32 0, i32 0
  %141 = extractvalue { ptr, ptr } %139, 0
  store ptr %141, ptr %140, align 8
  %142 = getelementptr inbounds { ptr, ptr }, ptr %27, i32 0, i32 1
  %143 = extractvalue { ptr, ptr } %139, 1
  store ptr %143, ptr %142, align 8
  %144 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSIN5Eigen7ProductINS1_9TransposeINS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEES5_Li0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(72) %19, ptr noundef nonnull align 1 dereferenceable(1) %27)
  call void @llvm.lifetime.end.p0(i64 8, ptr %28) #8
  call void @llvm.lifetime.end.p0(i64 16, ptr %27) #8
  call void @llvm.lifetime.start.p0(i64 24, ptr %29) #8
  %145 = load ptr, ptr %6, align 8, !tbaa !5
  %146 = load ptr, ptr %7, align 8, !tbaa !5
  %147 = load ptr, ptr %8, align 8, !tbaa !5
  %148 = getelementptr inbounds %"struct.RigidBodyDynamics::ConstraintSet", ptr %147, i32 0, i32 11
  %149 = load i32, ptr %14, align 4, !tbaa !13
  %150 = zext i32 %149 to i64
  %151 = call noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt6vectorIjSaIjEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %148, i64 noundef %150) #8
  %152 = load i32, ptr %151, align 4, !tbaa !13
  %153 = load ptr, ptr %8, align 8, !tbaa !5
  %154 = getelementptr inbounds %"struct.RigidBodyDynamics::ConstraintSet", ptr %153, i32 0, i32 13
  %155 = load i32, ptr %14, align 4, !tbaa !13
  %156 = zext i32 %155 to i64
  %157 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %154, i64 noundef %156) #8
  %158 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %157, i32 0, i32 1
  call void @_ZN17RigidBodyDynamics25CalcBodyToBaseCoordinatesERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEjRK9Vector3_tb(ptr dead_on_unwind writable sret(%class.Vector3_t) align 8 %29, ptr noundef nonnull align 8 dereferenceable(872) %145, ptr noundef nonnull align 8 dereferenceable(16) %146, i32 noundef %152, ptr noundef nonnull align 8 dereferenceable(24) %158, i1 noundef zeroext false)
  %159 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(24) %15, ptr noundef nonnull align 8 dereferenceable(24) %29) #8
  call void @llvm.lifetime.end.p0(i64 24, ptr %29) #8
  call void @llvm.lifetime.start.p0(i64 24, ptr %30) #8
  %160 = load ptr, ptr %6, align 8, !tbaa !5
  %161 = load ptr, ptr %7, align 8, !tbaa !5
  %162 = load ptr, ptr %8, align 8, !tbaa !5
  %163 = getelementptr inbounds %"struct.RigidBodyDynamics::ConstraintSet", ptr %162, i32 0, i32 12
  %164 = load i32, ptr %14, align 4, !tbaa !13
  %165 = zext i32 %164 to i64
  %166 = call noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt6vectorIjSaIjEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %163, i64 noundef %165) #8
  %167 = load i32, ptr %166, align 4, !tbaa !13
  %168 = load ptr, ptr %8, align 8, !tbaa !5
  %169 = getelementptr inbounds %"struct.RigidBodyDynamics::ConstraintSet", ptr %168, i32 0, i32 14
  %170 = load i32, ptr %14, align 4, !tbaa !13
  %171 = zext i32 %170 to i64
  %172 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %169, i64 noundef %171) #8
  %173 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %172, i32 0, i32 1
  call void @_ZN17RigidBodyDynamics25CalcBodyToBaseCoordinatesERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEjRK9Vector3_tb(ptr dead_on_unwind writable sret(%class.Vector3_t) align 8 %30, ptr noundef nonnull align 8 dereferenceable(872) %160, ptr noundef nonnull align 8 dereferenceable(16) %161, i32 noundef %167, ptr noundef nonnull align 8 dereferenceable(24) %173, i1 noundef zeroext false)
  %174 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(24) %16, ptr noundef nonnull align 8 dereferenceable(24) %30) #8
  call void @llvm.lifetime.end.p0(i64 24, ptr %30) #8
  %175 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %19, i64 noundef 1, i64 noundef 2)
  %176 = load double, ptr %175, align 8, !tbaa !15
  %177 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %19, i64 noundef 2, i64 noundef 1)
  %178 = load double, ptr %177, align 8, !tbaa !15
  %179 = fsub double %176, %178
  %180 = fmul double -5.000000e-01, %179
  %181 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %20, i64 noundef 0)
  store double %180, ptr %181, align 8, !tbaa !15
  %182 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %19, i64 noundef 2, i64 noundef 0)
  %183 = load double, ptr %182, align 8, !tbaa !15
  %184 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %19, i64 noundef 0, i64 noundef 2)
  %185 = load double, ptr %184, align 8, !tbaa !15
  %186 = fsub double %183, %185
  %187 = fmul double -5.000000e-01, %186
  %188 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %20, i64 noundef 1)
  store double %187, ptr %188, align 8, !tbaa !15
  %189 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %19, i64 noundef 0, i64 noundef 1)
  %190 = load double, ptr %189, align 8, !tbaa !15
  %191 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %19, i64 noundef 1, i64 noundef 0)
  %192 = load double, ptr %191, align 8, !tbaa !15
  %193 = fsub double %190, %192
  %194 = fmul double -5.000000e-01, %193
  %195 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %20, i64 noundef 2)
  store double %194, ptr %195, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 32, ptr %31) #8
  call void @llvm.lifetime.start.p0(i64 8, ptr %32) #8
  %196 = call ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1) %17)
  %197 = getelementptr inbounds %"class.Eigen::Transpose.397", ptr %32, i32 0, i32 0
  store ptr %196, ptr %197, align 8
  call void @llvm.lifetime.start.p0(i64 24, ptr %33) #8
  call void @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEmiIS2_EEKNS_13CwiseBinaryOpINS_8internal20scalar_difference_opIdNS6_6traitsIT_E6ScalarEEEKS2_KS9_EERKNS0_IS9_EE(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseBinaryOp.412") align 8 %33, ptr noundef nonnull align 1 dereferenceable(1) %16, ptr noundef nonnull align 1 dereferenceable(1) %15)
  call void @_ZNK5Eigen10MatrixBaseINS_9TransposeINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEmlINS_13CwiseBinaryOpINS_8internal20scalar_difference_opIddEEKNS2_IdLi3ELi1ELi0ELi3ELi1EEESC_EEEEKNS_7ProductIS4_T_Li0EEERKNS0_ISF_EE(ptr dead_on_unwind writable sret(%"class.Eigen::Product.405") align 8 %31, ptr noundef nonnull align 1 dereferenceable(1) %32, ptr noundef nonnull align 1 dereferenceable(1) %33)
  call void @llvm.lifetime.start.p0(i64 48, ptr %34) #8
  call void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEE5blockILi3ELi1EEENS3_13FixedBlockXprIXT_EXT0_EE4TypeEll(ptr dead_on_unwind writable sret(%"class.Eigen::Block.419") align 8 %34, ptr noundef nonnull align 1 dereferenceable(1) %20, i64 noundef 3, i64 noundef 0)
  %198 = call noundef nonnull align 8 dereferenceable(48) ptr @_ZN5Eigen5BlockINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi3ELi1ELb0EEaSINS_7ProductINS_9TransposeINS1_IdLi3ELi3ELi0ELi3ELi3EEEEENS_13CwiseBinaryOpINS_8internal20scalar_difference_opIddEEKNS1_IdLi3ELi1ELi0ELi3ELi1EEESE_EELi0EEEEERS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(48) %34, ptr noundef nonnull align 1 dereferenceable(1) %31)
  call void @llvm.lifetime.end.p0(i64 48, ptr %34) #8
  call void @llvm.lifetime.end.p0(i64 24, ptr %33) #8
  call void @llvm.lifetime.end.p0(i64 8, ptr %32) #8
  call void @llvm.lifetime.end.p0(i64 32, ptr %31) #8
  call void @llvm.lifetime.start.p0(i64 16, ptr %35) #8
  call void @llvm.lifetime.start.p0(i64 8, ptr %36) #8
  %199 = load ptr, ptr %8, align 8, !tbaa !5
  %200 = getelementptr inbounds %"struct.RigidBodyDynamics::ConstraintSet", ptr %199, i32 0, i32 15
  %201 = load i32, ptr %14, align 4, !tbaa !13
  %202 = zext i32 %201 to i64
  %203 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %200, i64 noundef %202) #8
  %204 = call ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1) %203)
  %205 = getelementptr inbounds %"class.Eigen::Transpose.438", ptr %36, i32 0, i32 0
  store ptr %204, ptr %205, align 8
  %206 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_9TransposeINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEEEEmlIS3_EEKNS_7ProductIS4_T_Li0EEERKNS0_IS8_EE(ptr noundef nonnull align 1 dereferenceable(1) %36, ptr noundef nonnull align 1 dereferenceable(1) %20)
  %207 = getelementptr inbounds { ptr, ptr }, ptr %35, i32 0, i32 0
  %208 = extractvalue { ptr, ptr } %206, 0
  store ptr %208, ptr %207, align 8
  %209 = getelementptr inbounds { ptr, ptr }, ptr %35, i32 0, i32 1
  %210 = extractvalue { ptr, ptr } %206, 1
  store ptr %210, ptr %209, align 8
  %211 = call noundef double @_ZNK5Eigen8internal18dense_product_baseINS_9TransposeINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEEES4_Li0ELi6EEcvKdEv(ptr noundef nonnull align 1 dereferenceable(1) %35)
  %212 = load ptr, ptr %9, align 8, !tbaa !5
  %213 = load i32, ptr %14, align 4, !tbaa !13
  %214 = zext i32 %213 to i64
  %215 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %212, i64 noundef %214)
  store double %211, ptr %215, align 8, !tbaa !15
  call void @llvm.lifetime.end.p0(i64 8, ptr %36) #8
  call void @llvm.lifetime.end.p0(i64 16, ptr %35) #8
  call void @llvm.lifetime.end.p0(i64 48, ptr %20) #8
  call void @llvm.lifetime.end.p0(i64 72, ptr %19) #8
  call void @llvm.lifetime.end.p0(i64 72, ptr %18) #8
  call void @llvm.lifetime.end.p0(i64 72, ptr %17) #8
  call void @llvm.lifetime.end.p0(i64 24, ptr %16) #8
  call void @llvm.lifetime.end.p0(i64 24, ptr %15) #8
  call void @llvm.lifetime.end.p0(i64 4, ptr %14) #8
  br label %216

216:                                              ; preds = %86
  %217 = load i32, ptr %13, align 4, !tbaa !13
  %218 = add i32 %217, 1
  store i32 %218, ptr %13, align 4, !tbaa !13
  br label %78, !llvm.loop !19

219:                                              ; preds = %85
  call void @llvm.lifetime.start.p0(i64 4, ptr %37) #8
  store i32 0, ptr %37, align 4, !tbaa !13
  br label %220

220:                                              ; preds = %250, %219
  %221 = load i32, ptr %37, align 4, !tbaa !13
  %222 = zext i32 %221 to i64
  %223 = load ptr, ptr %8, align 8, !tbaa !5
  %224 = getelementptr inbounds %"struct.RigidBodyDynamics::ConstraintSet", ptr %223, i32 0, i32 6
  %225 = call noundef i64 @_ZNKSt6vectorIjSaIjEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24) %224) #8
  %226 = icmp ult i64 %222, %225
  br i1 %226, label %228, label %227

227:                                              ; preds = %220
  call void @llvm.lifetime.end.p0(i64 4, ptr %37) #8
  br label %253

228:                                              ; preds = %220
  call void @llvm.lifetime.start.p0(i64 4, ptr %38) #8
  %229 = load ptr, ptr %8, align 8, !tbaa !5
  %230 = getelementptr inbounds %"struct.RigidBodyDynamics::ConstraintSet", ptr %229, i32 0, i32 6
  %231 = load i32, ptr %37, align 4, !tbaa !13
  %232 = zext i32 %231 to i64
  %233 = call noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt6vectorIjSaIjEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %230, i64 noundef %232) #8
  %234 = load i32, ptr %233, align 4, !tbaa !13
  store i32 %234, ptr %38, align 4, !tbaa !13
  %235 = load ptr, ptr %8, align 8, !tbaa !5
  %236 = getelementptr inbounds %"struct.RigidBodyDynamics::ConstraintSet", ptr %235, i32 0, i32 7
  %237 = load i32, ptr %37, align 4, !tbaa !13
  %238 = zext i32 %237 to i64
  %239 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSt6vectorIPN17RigidBodyDynamics16CustomConstraintESaIS2_EEixEm(ptr noundef nonnull align 8 dereferenceable(24) %236, i64 noundef %238) #8
  %240 = load ptr, ptr %239, align 8, !tbaa !5
  %241 = load ptr, ptr %6, align 8, !tbaa !5
  %242 = load i32, ptr %38, align 4, !tbaa !13
  %243 = load ptr, ptr %7, align 8, !tbaa !5
  %244 = load ptr, ptr %8, align 8, !tbaa !5
  %245 = load ptr, ptr %9, align 8, !tbaa !5
  %246 = load i32, ptr %38, align 4, !tbaa !13
  %247 = load ptr, ptr %240, align 8, !tbaa !20
  %248 = getelementptr inbounds ptr, ptr %247, i64 4
  %249 = load ptr, ptr %248, align 8
  call void %249(ptr noundef nonnull align 8 dereferenceable(12) %240, ptr noundef nonnull align 8 dereferenceable(872) %241, i32 noundef %242, ptr noundef nonnull align 8 dereferenceable(16) %243, ptr noundef nonnull align 8 dereferenceable(1160) %244, ptr noundef nonnull align 8 dereferenceable(16) %245, i32 noundef %246)
  call void @llvm.lifetime.end.p0(i64 4, ptr %38) #8
  br label %250

250:                                              ; preds = %228
  %251 = load i32, ptr %37, align 4, !tbaa !13
  %252 = add i32 %251, 1
  store i32 %252, ptr %37, align 4, !tbaa !13
  br label %220, !llvm.loop !22

253:                                              ; preds = %227
  ret void
}

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

declare void @_ZN17RigidBodyDynamics22UpdateKinematicsCustomERNS_5ModelEPKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEES6_S6_(ptr noundef nonnull align 8 dereferenceable(872), ptr noundef, ptr noundef, ptr noundef) #5

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

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

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

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN9Matrix3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(72)) unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN15SpatialVector_tC2Ev(ptr noundef nonnull align 16 dereferenceable(48)) unnamed_addr #3 align 2

declare void @_ZN17RigidBodyDynamics24CalcBodyWorldOrientationERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEjb(ptr dead_on_unwind writable sret(%class.Matrix3_t) align 8, ptr noundef nonnull align 8 dereferenceable(872), ptr noundef nonnull align 8 dereferenceable(16), i32 noundef, i1 noundef zeroext) #5

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

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

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

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSIN5Eigen7ProductINS1_9TransposeINS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEES5_Li0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(72), ptr noundef nonnull align 1 dereferenceable(1)) #2 align 2

declare void @_ZN17RigidBodyDynamics25CalcBodyToBaseCoordinatesERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEjRK9Vector3_tb(ptr dead_on_unwind writable sret(%class.Vector3_t) align 8, ptr noundef nonnull align 8 dereferenceable(872), ptr noundef nonnull align 8 dereferenceable(16), i32 noundef, ptr noundef nonnull align 8 dereferenceable(24), i1 noundef zeroext) #5

; Function Attrs: inlinehint mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(24)) #6 align 2

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

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

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZNK5Eigen10MatrixBaseINS_9TransposeINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEmlINS_13CwiseBinaryOpINS_8internal20scalar_difference_opIddEEKNS2_IdLi3ELi1ELi0ELi3ELi1EEESC_EEEEKNS_7ProductIS4_T_Li0EEERKNS0_ISF_EE(ptr dead_on_unwind noalias writable sret(%"class.Eigen::Product.405") align 8, 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 void @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEmiIS2_EEKNS_13CwiseBinaryOpINS_8internal20scalar_difference_opIdNS6_6traitsIT_E6ScalarEEEKS2_KS9_EERKNS0_IS9_EE(ptr dead_on_unwind noalias writable sret(%"class.Eigen::CwiseBinaryOp.412") align 8, 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 void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEE5blockILi3ELi1EEENS3_13FixedBlockXprIXT_EXT0_EE4TypeEll(ptr dead_on_unwind noalias writable sret(%"class.Eigen::Block.419") align 8, ptr noundef nonnull align 1 dereferenceable(1), i64 noundef, i64 noundef) #3 align 2

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

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

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_9TransposeINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEEEEmlIS3_EEKNS_7ProductIS4_T_Li0EEERKNS0_IS8_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 @_ZNK5Eigen8internal18dense_product_baseINS_9TransposeINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEEES4_Li0ELi6EEcvKdEv(ptr noundef nonnull align 1 dereferenceable(1)) #3 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZNSt6vectorIPN17RigidBodyDynamics16CustomConstraintESaIS2_EEixEm(ptr noundef nonnull align 8 dereferenceable(24), i64 noundef) #4 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 = { 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 = { "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 = { 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 #7 = { noreturn nounwind }
attributes #8 = { 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 = !{!"bool", !7, i64 0}
!11 = !{i8 0, i8 2}
!12 = !{}
!13 = !{!14, !14, i64 0}
!14 = !{!"int", !7, i64 0}
!15 = !{!16, !16, i64 0}
!16 = !{!"double", !7, i64 0}
!17 = distinct !{!17, !18}
!18 = !{!"llvm.loop.mustprogress"}
!19 = distinct !{!19, !18}
!20 = !{!21, !21, i64 0}
!21 = !{!"vtable pointer", !8, i64 0}
!22 = distinct !{!22, !18}


; 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.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.Matrix3_t = type { %"class.Eigen::Matrix.79" }
%"class.Eigen::Matrix.79" = type { %"class.Eigen::PlainObjectBase.80" }
%"class.Eigen::PlainObjectBase.80" = type { %"class.Eigen::DenseStorage.87" }
%"class.Eigen::DenseStorage.87" = type { %"struct.Eigen::internal::plain_array.88" }
%"struct.Eigen::internal::plain_array.88" = type { [9 x double] }
%class.SpatialVector_t = type { %"class.Eigen::Matrix.89" }
%"class.Eigen::Matrix.89" = type { %"class.Eigen::PlainObjectBase.90" }
%"class.Eigen::PlainObjectBase.90" = type { %"class.Eigen::DenseStorage.97" }
%"class.Eigen::DenseStorage.97" = type { %"struct.Eigen::internal::plain_array.98" }
%"struct.Eigen::internal::plain_array.98" = type { [6 x double] }
%"class.Eigen::Product.390" = type { %"class.Eigen::Transpose.397", ptr }
%"class.Eigen::Transpose.397" = type { ptr }
%"class.Eigen::Product.405" = type { %"class.Eigen::Transpose.397", %"class.Eigen::CwiseBinaryOp.412" }
%"class.Eigen::CwiseBinaryOp.412" = type <{ ptr, ptr, [8 x i8] }>
%"class.Eigen::Block.419" = type { %"class.Eigen::BlockImpl.420" }
%"class.Eigen::BlockImpl.420" = type { %"class.Eigen::internal::BlockImpl_dense.421" }
%"class.Eigen::internal::BlockImpl_dense.421" = type { %"class.Eigen::MapBase.base.430", ptr, %"class.Eigen::internal::variable_if_dynamic", [8 x i8], i64 }
%"class.Eigen::MapBase.base.430" = type { %"class.Eigen::MapBase.base" }
%"class.Eigen::MapBase.base" = type <{ ptr, [2 x i8] }>
%"class.Eigen::internal::variable_if_dynamic" = type { i64 }
%"class.Eigen::Product.431" = type { %"class.Eigen::Transpose.438", ptr }
%"class.Eigen::Transpose.438" = type { ptr }

@.str.4 = external hidden unnamed_addr constant [112 x i8], align 1
@.str.9 = external hidden unnamed_addr constant [24 x i8], align 1
@__PRETTY_FUNCTION__._ZN17RigidBodyDynamics28CalcConstraintsPositionErrorERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_b = external hidden unnamed_addr constant [127 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

; 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: mustprogress nounwind uwtable
declare dso_local noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24), i64 noundef) local_unnamed_addr #4 align 2

; Function Attrs: mustprogress uwtable
define dso_local 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) %3, i1 noundef zeroext %4) local_unnamed_addr #2 {
  %6 = alloca %class.Vector3_t, align 8
  %7 = alloca %class.Vector3_t, align 8
  %8 = alloca %class.Matrix3_t, align 8
  %9 = alloca %class.Matrix3_t, align 8
  %10 = alloca %class.Matrix3_t, align 8
  %11 = alloca %class.SpatialVector_t, align 16
  %12 = alloca %"class.Eigen::Product.390", align 8
  %13 = alloca %"class.Eigen::Transpose.397", align 8
  %14 = alloca %class.Matrix3_t, align 8
  %15 = alloca %"class.Eigen::Product.390", align 8
  %16 = alloca %"class.Eigen::Transpose.397", align 8
  %17 = alloca %class.Matrix3_t, align 8
  %18 = alloca %"class.Eigen::Product.390", align 8
  %19 = alloca %"class.Eigen::Transpose.397", align 8
  %20 = alloca %class.Vector3_t, align 8
  %21 = alloca %class.Vector3_t, align 8
  %22 = alloca %"class.Eigen::Product.405", align 8
  %23 = alloca %"class.Eigen::Transpose.397", align 8
  %24 = alloca %"class.Eigen::CwiseBinaryOp.412", align 8
  %25 = alloca %"class.Eigen::Block.419", align 8
  %26 = alloca %"class.Eigen::Product.431", align 8
  %27 = alloca %"class.Eigen::Transpose.438", align 8
  %28 = tail call noundef i64 @_ZNK5Eigen9EigenBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE4sizeEv(ptr noundef nonnull align 1 dereferenceable(1) %3)
  %29 = tail call noundef i64 @_ZNK17RigidBodyDynamics13ConstraintSet4sizeEv(ptr noundef nonnull align 8 dereferenceable(1160) %2)
  %30 = icmp eq i64 %28, %29
  br i1 %30, label %32, label %31

31:                                               ; preds = %5
  tail call void @__assert_fail(ptr noundef nonnull @.str.9, ptr noundef nonnull @.str.4, i32 noundef 490, ptr noundef nonnull @__PRETTY_FUNCTION__._ZN17RigidBodyDynamics28CalcConstraintsPositionErrorERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERNS_13ConstraintSetERS4_b) #7
  unreachable

32:                                               ; preds = %5
  br i1 %4, label %33, label %34

33:                                               ; preds = %32
  tail call void @_ZN17RigidBodyDynamics22UpdateKinematicsCustomERNS_5ModelEPKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEES6_S6_(ptr noundef nonnull align 8 dereferenceable(872) %0, ptr noundef nonnull %1, ptr noundef null, ptr noundef null)
  br label %34

34:                                               ; preds = %33, %32
  %35 = getelementptr inbounds i8, ptr %2, i64 56
  %36 = tail call noundef i64 @_ZNKSt6vectorIjSaIjEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24) %35) #8
  %.not = icmp eq i64 %36, 0
  br i1 %.not, label %.preheader62, label %.lr.ph

.preheader62:                                     ; preds = %.lr.ph, %34
  %37 = getelementptr inbounds i8, ptr %2, i64 80
  %38 = tail call noundef i64 @_ZNKSt6vectorIjSaIjEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24) %37) #8
  %.not68 = icmp eq i64 %38, 0
  br i1 %.not68, label %.preheader, label %.lr.ph65

.lr.ph65:                                         ; preds = %.preheader62
  %39 = getelementptr inbounds i8, ptr %2, i64 224
  %40 = getelementptr inbounds i8, ptr %2, i64 272
  %41 = getelementptr inbounds i8, ptr %12, i64 8
  %42 = getelementptr inbounds i8, ptr %2, i64 248
  %43 = getelementptr inbounds i8, ptr %2, i64 296
  %44 = getelementptr inbounds i8, ptr %15, i64 8
  %45 = getelementptr inbounds i8, ptr %18, i64 8
  %46 = getelementptr inbounds i8, ptr %2, i64 320
  %47 = getelementptr inbounds i8, ptr %26, i64 8
  br label %60

.lr.ph:                                           ; preds = %34, %.lr.ph
  %48 = phi i64 [ %54, %.lr.ph ], [ 0, %34 ]
  %.063 = phi i32 [ %53, %.lr.ph ], [ 0, %34 ]
  %49 = tail call noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt6vectorIjSaIjEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %35, i64 noundef %48) #8
  %50 = load i32, ptr %49, align 4, !tbaa !5
  %51 = zext i32 %50 to i64
  %52 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %51)
  store double 0.000000e+00, ptr %52, align 8, !tbaa !9
  %53 = add i32 %.063, 1
  %54 = zext i32 %53 to i64
  %55 = tail call noundef i64 @_ZNKSt6vectorIjSaIjEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24) %35) #8
  %56 = icmp ugt i64 %55, %54
  br i1 %56, label %.lr.ph, label %.preheader62, !llvm.loop !11

.preheader:                                       ; preds = %60, %.preheader62
  %57 = getelementptr inbounds i8, ptr %2, i64 104
  %58 = call noundef i64 @_ZNKSt6vectorIjSaIjEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24) %57) #8
  %.not69 = icmp eq i64 %58, 0
  br i1 %.not69, label %._crit_edge, label %.lr.ph67

.lr.ph67:                                         ; preds = %.preheader
  %59 = getelementptr inbounds i8, ptr %2, i64 128
  br label %130

60:                                               ; preds = %.lr.ph65, %60
  %61 = phi i64 [ 0, %.lr.ph65 ], [ %127, %60 ]
  %.05864 = phi i32 [ 0, %.lr.ph65 ], [ %126, %60 ]
  %62 = call noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt6vectorIjSaIjEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %37, i64 noundef %61) #8
  %63 = load i32, ptr %62, align 4, !tbaa !5
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %6) #8
  call void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %6)
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %7) #8
  call void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %7)
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %8) #8
  call void @_ZN9Matrix3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(72) %8)
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %9) #8
  call void @_ZN9Matrix3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(72) %9)
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %10) #8
  call void @_ZN9Matrix3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(72) %10)
  call void @llvm.lifetime.start.p0(i64 48, ptr nonnull %11) #8
  call void @_ZN15SpatialVector_tC2Ev(ptr noundef nonnull align 16 dereferenceable(48) %11)
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %12) #8
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %13) #8
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %14) #8
  %64 = zext i32 %63 to i64
  %65 = call noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt6vectorIjSaIjEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %39, i64 noundef %64) #8
  %66 = load i32, ptr %65, align 4, !tbaa !5
  call void @_ZN17RigidBodyDynamics24CalcBodyWorldOrientationERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEjb(ptr dead_on_unwind nonnull writable sret(%class.Matrix3_t) align 8 %14, ptr noundef nonnull align 8 dereferenceable(872) %0, ptr noundef nonnull align 8 dereferenceable(16) %1, i32 noundef %66, i1 noundef zeroext false)
  %67 = call ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1) %14)
  store ptr %67, ptr %13, align 8
  %68 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %40, i64 noundef %64) #8
  %69 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_9TransposeINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEmlIS3_EEKNS_7ProductIS4_T_Li0EEERKNS0_IS8_EE(ptr noundef nonnull align 1 dereferenceable(1) %13, ptr noundef nonnull align 1 dereferenceable(1) %68)
  %70 = extractvalue { ptr, ptr } %69, 0
  store ptr %70, ptr %12, align 8
  %71 = extractvalue { ptr, ptr } %69, 1
  store ptr %71, ptr %41, align 8
  %72 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSIN5Eigen7ProductINS1_9TransposeINS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEES5_Li0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(72) %8, ptr noundef nonnull align 1 dereferenceable(1) %12)
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %14) #8
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %13) #8
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %12) #8
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %15) #8
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %16) #8
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %17) #8
  %73 = call noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt6vectorIjSaIjEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %42, i64 noundef %64) #8
  %74 = load i32, ptr %73, align 4, !tbaa !5
  call void @_ZN17RigidBodyDynamics24CalcBodyWorldOrientationERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEjb(ptr dead_on_unwind nonnull writable sret(%class.Matrix3_t) align 8 %17, ptr noundef nonnull align 8 dereferenceable(872) %0, ptr noundef nonnull align 8 dereferenceable(16) %1, i32 noundef %74, i1 noundef zeroext false)
  %75 = call ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1) %17)
  store ptr %75, ptr %16, align 8
  %76 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %43, i64 noundef %64) #8
  %77 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_9TransposeINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEmlIS3_EEKNS_7ProductIS4_T_Li0EEERKNS0_IS8_EE(ptr noundef nonnull align 1 dereferenceable(1) %16, ptr noundef nonnull align 1 dereferenceable(1) %76)
  %78 = extractvalue { ptr, ptr } %77, 0
  store ptr %78, ptr %15, align 8
  %79 = extractvalue { ptr, ptr } %77, 1
  store ptr %79, ptr %44, align 8
  %80 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSIN5Eigen7ProductINS1_9TransposeINS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEES5_Li0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(72) %9, ptr noundef nonnull align 1 dereferenceable(1) %15)
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %17) #8
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %16) #8
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %15) #8
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %18) #8
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %19) #8
  %81 = call ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1) %8)
  store ptr %81, ptr %19, align 8
  %82 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_9TransposeINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEmlIS3_EEKNS_7ProductIS4_T_Li0EEERKNS0_IS8_EE(ptr noundef nonnull align 1 dereferenceable(1) %19, ptr noundef nonnull align 1 dereferenceable(1) %9)
  %83 = extractvalue { ptr, ptr } %82, 0
  store ptr %83, ptr %18, align 8
  %84 = extractvalue { ptr, ptr } %82, 1
  store ptr %84, ptr %45, align 8
  %85 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSIN5Eigen7ProductINS1_9TransposeINS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEES5_Li0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(72) %10, ptr noundef nonnull align 1 dereferenceable(1) %18)
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %19) #8
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %18) #8
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %20) #8
  %86 = call noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt6vectorIjSaIjEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %39, i64 noundef %64) #8
  %87 = load i32, ptr %86, align 4, !tbaa !5
  %88 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %40, i64 noundef %64) #8
  %89 = getelementptr inbounds i8, ptr %88, i64 72
  call void @_ZN17RigidBodyDynamics25CalcBodyToBaseCoordinatesERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEjRK9Vector3_tb(ptr dead_on_unwind nonnull writable sret(%class.Vector3_t) align 8 %20, ptr noundef nonnull align 8 dereferenceable(872) %0, ptr noundef nonnull align 8 dereferenceable(16) %1, i32 noundef %87, ptr noundef nonnull align 8 dereferenceable(24) %89, i1 noundef zeroext false)
  %90 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(24) %6, ptr noundef nonnull align 8 dereferenceable(24) %20) #8
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %20) #8
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %21) #8
  %91 = call noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt6vectorIjSaIjEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %42, i64 noundef %64) #8
  %92 = load i32, ptr %91, align 4, !tbaa !5
  %93 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %43, i64 noundef %64) #8
  %94 = getelementptr inbounds i8, ptr %93, i64 72
  call void @_ZN17RigidBodyDynamics25CalcBodyToBaseCoordinatesERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEjRK9Vector3_tb(ptr dead_on_unwind nonnull writable sret(%class.Vector3_t) align 8 %21, ptr noundef nonnull align 8 dereferenceable(872) %0, ptr noundef nonnull align 8 dereferenceable(16) %1, i32 noundef %92, ptr noundef nonnull align 8 dereferenceable(24) %94, i1 noundef zeroext false)
  %95 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(24) %7, ptr noundef nonnull align 8 dereferenceable(24) %21) #8
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %21) #8
  %96 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %10, i64 noundef 1, i64 noundef 2)
  %97 = load double, ptr %96, align 8, !tbaa !9
  %98 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %10, i64 noundef 2, i64 noundef 1)
  %99 = load double, ptr %98, align 8, !tbaa !9
  %100 = fsub double %97, %99
  %101 = fmul double %100, -5.000000e-01
  %102 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %11, i64 noundef 0)
  store double %101, ptr %102, align 8, !tbaa !9
  %103 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %10, i64 noundef 2, i64 noundef 0)
  %104 = load double, ptr %103, align 8, !tbaa !9
  %105 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %10, i64 noundef 0, i64 noundef 2)
  %106 = load double, ptr %105, align 8, !tbaa !9
  %107 = fsub double %104, %106
  %108 = fmul double %107, -5.000000e-01
  %109 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %11, i64 noundef 1)
  store double %108, ptr %109, align 8, !tbaa !9
  %110 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %10, i64 noundef 0, i64 noundef 1)
  %111 = load double, ptr %110, align 8, !tbaa !9
  %112 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %10, i64 noundef 1, i64 noundef 0)
  %113 = load double, ptr %112, align 8, !tbaa !9
  %114 = fsub double %111, %113
  %115 = fmul double %114, -5.000000e-01
  %116 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %11, i64 noundef 2)
  store double %115, ptr %116, align 8, !tbaa !9
  call void @llvm.lifetime.start.p0(i64 32, ptr nonnull %22) #8
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %23) #8
  %117 = call ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1) %8)
  store ptr %117, ptr %23, align 8
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %24) #8
  call void @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEmiIS2_EEKNS_13CwiseBinaryOpINS_8internal20scalar_difference_opIdNS6_6traitsIT_E6ScalarEEEKS2_KS9_EERKNS0_IS9_EE(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseBinaryOp.412") align 8 %24, ptr noundef nonnull align 1 dereferenceable(1) %7, ptr noundef nonnull align 1 dereferenceable(1) %6)
  call void @_ZNK5Eigen10MatrixBaseINS_9TransposeINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEmlINS_13CwiseBinaryOpINS_8internal20scalar_difference_opIddEEKNS2_IdLi3ELi1ELi0ELi3ELi1EEESC_EEEEKNS_7ProductIS4_T_Li0EEERKNS0_ISF_EE(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::Product.405") align 8 %22, ptr noundef nonnull align 1 dereferenceable(1) %23, ptr noundef nonnull align 1 dereferenceable(1) %24)
  call void @llvm.lifetime.start.p0(i64 48, ptr nonnull %25) #8
  call void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEE5blockILi3ELi1EEENS3_13FixedBlockXprIXT_EXT0_EE4TypeEll(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::Block.419") align 8 %25, ptr noundef nonnull align 1 dereferenceable(1) %11, i64 noundef 3, i64 noundef 0)
  %118 = call noundef nonnull align 8 dereferenceable(48) ptr @_ZN5Eigen5BlockINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi3ELi1ELb0EEaSINS_7ProductINS_9TransposeINS1_IdLi3ELi3ELi0ELi3ELi3EEEEENS_13CwiseBinaryOpINS_8internal20scalar_difference_opIddEEKNS1_IdLi3ELi1ELi0ELi3ELi1EEESE_EELi0EEEEERS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(48) %25, ptr noundef nonnull align 1 dereferenceable(1) %22)
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %25) #8
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %24) #8
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %23) #8
  call void @llvm.lifetime.end.p0(i64 32, ptr nonnull %22) #8
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %26) #8
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %27) #8
  %119 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %46, i64 noundef %64) #8
  %120 = call ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1) %119)
  store ptr %120, ptr %27, align 8
  %121 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_9TransposeINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEEEEmlIS3_EEKNS_7ProductIS4_T_Li0EEERKNS0_IS8_EE(ptr noundef nonnull align 1 dereferenceable(1) %27, ptr noundef nonnull align 1 dereferenceable(1) %11)
  %122 = extractvalue { ptr, ptr } %121, 0
  store ptr %122, ptr %26, align 8
  %123 = extractvalue { ptr, ptr } %121, 1
  store ptr %123, ptr %47, align 8
  %124 = call noundef double @_ZNK5Eigen8internal18dense_product_baseINS_9TransposeINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEEES4_Li0ELi6EEcvKdEv(ptr noundef nonnull align 1 dereferenceable(1) %26)
  %125 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %64)
  store double %124, ptr %125, align 8, !tbaa !9
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %27) #8
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %26) #8
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %11) #8
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %10) #8
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %9) #8
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %8) #8
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %7) #8
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %6) #8
  %126 = add i32 %.05864, 1
  %127 = zext i32 %126 to i64
  %128 = call noundef i64 @_ZNKSt6vectorIjSaIjEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24) %37) #8
  %129 = icmp ugt i64 %128, %127
  br i1 %129, label %60, label %.preheader, !llvm.loop !13

._crit_edge:                                      ; preds = %130, %.preheader
  ret void

130:                                              ; preds = %.lr.ph67, %130
  %131 = phi i64 [ 0, %.lr.ph67 ], [ %140, %130 ]
  %.05766 = phi i32 [ 0, %.lr.ph67 ], [ %139, %130 ]
  %132 = call noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt6vectorIjSaIjEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %57, i64 noundef %131) #8
  %133 = load i32, ptr %132, align 4, !tbaa !5
  %134 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSt6vectorIPN17RigidBodyDynamics16CustomConstraintESaIS2_EEixEm(ptr noundef nonnull align 8 dereferenceable(24) %59, i64 noundef %131) #8
  %135 = load ptr, ptr %134, align 8, !tbaa !14
  %136 = load ptr, ptr %135, align 8, !tbaa !16
  %137 = getelementptr inbounds i8, ptr %136, i64 32
  %138 = load ptr, ptr %137, align 8
  call void %138(ptr noundef nonnull align 8 dereferenceable(12) %135, ptr noundef nonnull align 8 dereferenceable(872) %0, i32 noundef %133, ptr noundef nonnull align 8 dereferenceable(16) %1, ptr noundef nonnull align 8 dereferenceable(1160) %2, ptr noundef nonnull align 8 dereferenceable(16) %3, i32 noundef %133)
  %139 = add i32 %.05766, 1
  %140 = zext i32 %139 to i64
  %141 = call noundef i64 @_ZNKSt6vectorIjSaIjEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24) %57) #8
  %142 = icmp ugt i64 %141, %140
  br i1 %142, label %130, label %._crit_edge, !llvm.loop !18
}

; 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

declare void @_ZN17RigidBodyDynamics22UpdateKinematicsCustomERNS_5ModelEPKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEES6_S6_(ptr noundef nonnull align 8 dereferenceable(872), ptr noundef, ptr noundef, ptr noundef) local_unnamed_addr #5

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

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

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

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN9Matrix3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(72)) unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN15SpatialVector_tC2Ev(ptr noundef nonnull align 16 dereferenceable(48)) unnamed_addr #3 align 2

declare void @_ZN17RigidBodyDynamics24CalcBodyWorldOrientationERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEjb(ptr dead_on_unwind writable sret(%class.Matrix3_t) align 8, ptr noundef nonnull align 8 dereferenceable(872), ptr noundef nonnull align 8 dereferenceable(16), i32 noundef, i1 noundef zeroext) local_unnamed_addr #5

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

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

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

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

declare void @_ZN17RigidBodyDynamics25CalcBodyToBaseCoordinatesERNS_5ModelERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEjRK9Vector3_tb(ptr dead_on_unwind writable sret(%class.Vector3_t) align 8, ptr noundef nonnull align 8 dereferenceable(872), ptr noundef nonnull align 8 dereferenceable(16), i32 noundef, ptr noundef nonnull align 8 dereferenceable(24), i1 noundef zeroext) local_unnamed_addr #5

; Function Attrs: inlinehint mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(24)) local_unnamed_addr #6 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(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 noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1), i64 noundef) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZNK5Eigen10MatrixBaseINS_9TransposeINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEmlINS_13CwiseBinaryOpINS_8internal20scalar_difference_opIddEEKNS2_IdLi3ELi1ELi0ELi3ELi1EEESC_EEEEKNS_7ProductIS4_T_Li0EEERKNS0_ISF_EE(ptr dead_on_unwind noalias writable sret(%"class.Eigen::Product.405") align 8, 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 void @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEmiIS2_EEKNS_13CwiseBinaryOpINS_8internal20scalar_difference_opIdNS6_6traitsIT_E6ScalarEEEKS2_KS9_EERKNS0_IS9_EE(ptr dead_on_unwind noalias writable sret(%"class.Eigen::CwiseBinaryOp.412") align 8, 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 void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEE5blockILi3ELi1EEENS3_13FixedBlockXprIXT_EXT0_EE4TypeEll(ptr dead_on_unwind noalias writable sret(%"class.Eigen::Block.419") align 8, 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 noundef nonnull align 8 dereferenceable(48) ptr @_ZN5Eigen5BlockINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi3ELi1ELb0EEaSINS_7ProductINS_9TransposeINS1_IdLi3ELi3ELi0ELi3ELi3EEEEENS_13CwiseBinaryOpINS_8internal20scalar_difference_opIddEEKNS1_IdLi3ELi1ELi0ELi3ELi1EEESE_EELi0EEEEERS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(48), ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #3 align 2

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

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_9TransposeINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEEEEmlIS3_EEKNS_7ProductIS4_T_Li0EEERKNS0_IS8_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 @_ZNK5Eigen8internal18dense_product_baseINS_9TransposeINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEEES4_Li0ELi6EEcvKdEv(ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #3 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZNSt6vectorIPN17RigidBodyDynamics16CustomConstraintESaIS2_EEixEm(ptr noundef nonnull align 8 dereferenceable(24), i64 noundef) local_unnamed_addr #4 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 = { 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 = { "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 = { 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 #7 = { noreturn nounwind }
attributes #8 = { 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 = !{!"int", !7, i64 0}
!7 = !{!"omnipotent char", !8, i64 0}
!8 = !{!"Simple C++ TBAA"}
!9 = !{!10, !10, i64 0}
!10 = !{!"double", !7, i64 0}
!11 = distinct !{!11, !12}
!12 = !{!"llvm.loop.mustprogress"}
!13 = distinct !{!13, !12}
!14 = !{!15, !15, i64 0}
!15 = !{!"any pointer", !7, i64 0}
!16 = !{!17, !17, i64 0}
!17 = !{!"vtable pointer", !8, i64 0}
!18 = distinct !{!18, !12}


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