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

; 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.Matrix3_t = type { %"class.Eigen::Matrix.90" }
%"class.Eigen::Matrix.90" = type { %"class.Eigen::PlainObjectBase.91" }
%"class.Eigen::PlainObjectBase.91" = type { %"class.Eigen::DenseStorage.98" }
%"class.Eigen::DenseStorage.98" = type { %"struct.Eigen::internal::plain_array.99" }
%"struct.Eigen::internal::plain_array.99" = type { [9 x double] }
%"struct.RigidBodyDynamics::Math::SpatialTransform" = type { %class.Matrix3_t, %class.Vector3_t }
%class.Vector3_t = type { %"class.Eigen::Matrix" }
%"class.Eigen::Matrix" = type { %"class.Eigen::PlainObjectBase" }
%"class.Eigen::PlainObjectBase" = type { %"class.Eigen::DenseStorage" }
%"class.Eigen::DenseStorage" = type { %"struct.Eigen::internal::plain_array" }
%"struct.Eigen::internal::plain_array" = type { [3 x double] }
%"class.Eigen::CwiseBinaryOp" = type <{ ptr, %"class.Eigen::CwiseNullaryOp", [8 x i8] }>
%"class.Eigen::CwiseNullaryOp" = type { [8 x i8], %"struct.Eigen::internal::scalar_constant_op" }
%"struct.Eigen::internal::scalar_constant_op" = type { double }
%"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.118" }
%"class.Eigen::MapBase.118" = type { ptr, %"class.Eigen::internal::variable_if_dynamic", %"class.Eigen::internal::variable_if_dynamic" }
%"class.Eigen::internal::variable_if_dynamic" = type { i64 }
%"class.Eigen::Product" = type { ptr, %"class.Eigen::Block" }
%class.SpatialVector_t = type { %"class.Eigen::Matrix.100" }
%"class.Eigen::Matrix.100" = type { %"class.Eigen::PlainObjectBase.101" }
%"class.Eigen::PlainObjectBase.101" = type { %"class.Eigen::DenseStorage.108" }
%"class.Eigen::DenseStorage.108" = type { %"struct.Eigen::internal::plain_array.109" }
%"struct.Eigen::internal::plain_array.109" = type { [6 x double] }
%"class.RigidBodyDynamics::Math::Quaternion" = type { %class.Vector4_t }
%class.Vector4_t = type { %"class.Eigen::Matrix.129" }
%"class.Eigen::Matrix.129" = type { %"class.Eigen::PlainObjectBase.130" }
%"class.Eigen::PlainObjectBase.130" = type { %"class.Eigen::DenseStorage.137" }
%"class.Eigen::DenseStorage.137" = type { %"struct.Eigen::internal::plain_array.138" }
%"struct.Eigen::internal::plain_array.138" = type { [4 x double] }
%"class.Eigen::CwiseNullaryOp.149" = type { [8 x i8], %"struct.Eigen::internal::scalar_constant_op" }
%"class.Eigen::Product.154" = type { ptr, ptr }
%"class.Eigen::CwiseBinaryOp.161" = type <{ ptr, %"class.Eigen::Product.167", [8 x i8] }>
%"class.Eigen::Product.167" = type { %"class.Eigen::Transpose", ptr }
%"class.Eigen::Transpose" = type { ptr }
%"struct.RigidBodyDynamics::Model" = type { %"class.std::vector", %"class.std::vector", %"class.std::vector.1", i32, i32, i32, i32, %class.Vector3_t, %"class.std::vector.8", %"class.std::vector.8", %"class.std::vector.14", %"class.std::vector.8", %"class.std::vector.8", %"class.std::vector.8", %"class.std::vector", %"class.std::vector.21", %"class.std::vector", %"class.std::vector.28", %"class.std::vector.28", %"class.std::vector.35", %"class.std::vector.40", %"class.std::vector", %"class.std::vector.45", %"class.std::vector.8", %"class.std::vector.50", %"class.std::vector.8", %"class.std::vector.8", %"class.Eigen::Matrix.57", %"class.Eigen::Matrix.57", %"class.std::vector.8", %"class.std::vector.66", %"class.std::vector.66", %"class.std::vector.8", %"class.std::vector.8", %"class.std::vector.21", %"class.std::vector.21", %"class.std::vector.73", i32, %"class.std::vector.80", %"class.std::map" }
%"class.std::vector.1" = type { %"struct.std::_Vector_base.2" }
%"struct.std::_Vector_base.2" = 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.14" = type { %"class.std::vector.15" }
%"class.std::vector.15" = type { %"struct.std::_Vector_base.16" }
%"struct.std::_Vector_base.16" = 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.28" = type { %"class.std::vector.29" }
%"class.std::vector.29" = type { %"struct.std::_Vector_base.30" }
%"struct.std::_Vector_base.30" = 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.35" = type { %"struct.std::_Vector_base.36" }
%"struct.std::_Vector_base.36" = 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.40" = type { %"struct.std::_Vector_base.41" }
%"struct.std::_Vector_base.41" = 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" = type { %"struct.std::_Vector_base" }
%"struct.std::_Vector_base" = 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.45" = type { %"struct.std::_Vector_base.46" }
%"struct.std::_Vector_base.46" = 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.50" = type { %"class.std::vector.51" }
%"class.std::vector.51" = type { %"struct.std::_Vector_base.52" }
%"struct.std::_Vector_base.52" = 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.Eigen::Matrix.57" = type { %"class.Eigen::PlainObjectBase.58" }
%"class.Eigen::PlainObjectBase.58" = type { %"class.Eigen::DenseStorage.65" }
%"class.Eigen::DenseStorage.65" = type { ptr, i64 }
%"class.std::vector.66" = type { %"class.std::vector.67" }
%"class.std::vector.67" = type { %"struct.std::_Vector_base.68" }
%"struct.std::_Vector_base.68" = 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.8" = type { %"class.std::vector.9" }
%"class.std::vector.9" = type { %"struct.std::_Vector_base.10" }
%"struct.std::_Vector_base.10" = 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.73" = type { %"class.std::vector.74" }
%"class.std::vector.74" = type { %"struct.std::_Vector_base.75" }
%"struct.std::_Vector_base.75" = 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.80" = type { %"class.std::vector.81" }
%"class.std::vector.81" = type { %"struct.std::_Vector_base.82" }
%"struct.std::_Vector_base.82" = 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::Joint" = type { ptr, i32, i32, i32, i32 }

@.str = external hidden unnamed_addr constant [13 x i8], align 1
@.str.4 = external hidden unnamed_addr constant [106 x i8], align 1
@__PRETTY_FUNCTION__._ZN17RigidBodyDynamics5jcalcERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEES6_ = external hidden unnamed_addr constant [89 x i8], align 1
@_ZSt4cerr = external global %"class.std::basic_ostream", align 8
@.str.5 = external hidden unnamed_addr constant [27 x i8], align 1
@.str.6 = external hidden unnamed_addr constant [8 x i8], align 1

; Function Attrs: mustprogress uwtable
define dso_local void @_ZN17RigidBodyDynamics5jcalcERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEES6_(ptr noundef nonnull align 8 dereferenceable(872) %0, i32 noundef %1, ptr noundef nonnull align 8 dereferenceable(16) %2, ptr noundef nonnull align 8 dereferenceable(16) %3) #0 {
  %5 = alloca ptr, align 8
  %6 = alloca i32, align 4
  %7 = alloca ptr, align 8
  %8 = alloca ptr, align 8
  %9 = alloca double, align 8
  %10 = alloca double, align 8
  %11 = alloca %class.Matrix3_t, align 8
  %12 = alloca double, align 8
  %13 = alloca double, align 8
  %14 = alloca double, align 8
  %15 = alloca double, align 8
  %16 = alloca double, align 8
  %17 = alloca double, align 8
  %18 = alloca double, align 8
  %19 = alloca double, align 8
  %20 = alloca %class.Matrix3_t, align 8
  %21 = alloca double, align 8
  %22 = alloca double, align 8
  %23 = alloca double, align 8
  %24 = alloca double, align 8
  %25 = alloca double, align 8
  %26 = alloca double, align 8
  %27 = alloca double, align 8
  %28 = alloca double, align 8
  %29 = alloca %class.Matrix3_t, align 8
  %30 = alloca double, align 8
  %31 = alloca double, align 8
  %32 = alloca double, align 8
  %33 = alloca double, align 8
  %34 = alloca double, align 8
  %35 = alloca double, align 8
  %36 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %37 = alloca double, align 8
  %38 = alloca %"class.Eigen::CwiseBinaryOp", align 8
  %39 = alloca %class.Vector3_t, align 8
  %40 = alloca %"class.Eigen::Block", align 8
  %41 = alloca %class.Vector3_t, align 8
  %42 = alloca %"class.Eigen::Product", align 8
  %43 = alloca %"class.Eigen::Block", align 8
  %44 = alloca %"class.Eigen::Matrix", align 8
  %45 = alloca double, align 8
  %46 = alloca %class.SpatialVector_t, align 16
  %47 = alloca double, align 8
  %48 = alloca double, align 8
  %49 = alloca double, align 8
  %50 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %51 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %52 = alloca %"class.Eigen::CwiseBinaryOp", align 8
  %53 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %54 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %55 = alloca %class.Matrix3_t, align 8
  %56 = alloca %"class.RigidBodyDynamics::Math::Quaternion", align 16
  %57 = alloca %class.Vector3_t, align 8
  %58 = alloca double, align 8
  %59 = alloca double, align 8
  %60 = alloca double, align 8
  %61 = alloca %class.Vector3_t, align 8
  %62 = alloca %class.SpatialVector_t, align 16
  %63 = alloca double, align 8
  %64 = alloca double, align 8
  %65 = alloca double, align 8
  %66 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %67 = alloca double, align 8
  %68 = alloca double, align 8
  %69 = alloca double, align 8
  %70 = alloca double, align 8
  %71 = alloca double, align 8
  %72 = alloca double, align 8
  %73 = alloca double, align 8
  %74 = alloca double, align 8
  %75 = alloca double, align 8
  %76 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %77 = alloca %class.Matrix3_t, align 8
  %78 = alloca double, align 8
  %79 = alloca double, align 8
  %80 = alloca double, align 8
  %81 = alloca double, align 8
  %82 = alloca double, align 8
  %83 = alloca double, align 8
  %84 = alloca double, align 8
  %85 = alloca double, align 8
  %86 = alloca double, align 8
  %87 = alloca %class.Vector3_t, align 8
  %88 = alloca %"class.Eigen::CwiseNullaryOp.149", align 8
  %89 = alloca double, align 8
  %90 = alloca double, align 8
  %91 = alloca double, align 8
  %92 = alloca %"class.Eigen::Product.154", align 8
  %93 = alloca %class.Vector3_t, align 8
  %94 = alloca double, align 8
  %95 = alloca double, align 8
  %96 = alloca double, align 8
  %97 = alloca double, align 8
  %98 = alloca double, align 8
  %99 = alloca double, align 8
  %100 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %101 = alloca double, align 8
  %102 = alloca double, align 8
  %103 = alloca double, align 8
  %104 = alloca double, align 8
  %105 = alloca double, align 8
  %106 = alloca double, align 8
  %107 = alloca double, align 8
  %108 = alloca double, align 8
  %109 = alloca double, align 8
  %110 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %111 = alloca %class.Matrix3_t, align 8
  %112 = alloca double, align 8
  %113 = alloca double, align 8
  %114 = alloca double, align 8
  %115 = alloca double, align 8
  %116 = alloca double, align 8
  %117 = alloca double, align 8
  %118 = alloca double, align 8
  %119 = alloca double, align 8
  %120 = alloca %class.Vector3_t, align 8
  %121 = alloca %"class.Eigen::CwiseNullaryOp.149", align 8
  %122 = alloca double, align 8
  %123 = alloca double, align 8
  %124 = alloca double, align 8
  %125 = alloca %"class.Eigen::Product.154", align 8
  %126 = alloca %class.Vector3_t, align 8
  %127 = alloca double, align 8
  %128 = alloca double, align 8
  %129 = alloca double, align 8
  %130 = alloca double, align 8
  %131 = alloca double, align 8
  %132 = alloca double, align 8
  %133 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %134 = alloca double, align 8
  %135 = alloca double, align 8
  %136 = alloca double, align 8
  %137 = alloca double, align 8
  %138 = alloca double, align 8
  %139 = alloca double, align 8
  %140 = alloca double, align 8
  %141 = alloca double, align 8
  %142 = alloca double, align 8
  %143 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %144 = alloca %class.Matrix3_t, align 8
  %145 = alloca double, align 8
  %146 = alloca double, align 8
  %147 = alloca double, align 8
  %148 = alloca double, align 8
  %149 = alloca double, align 8
  %150 = alloca double, align 8
  %151 = alloca double, align 8
  %152 = alloca double, align 8
  %153 = alloca double, align 8
  %154 = alloca %class.Vector3_t, align 8
  %155 = alloca %"class.Eigen::CwiseNullaryOp.149", align 8
  %156 = alloca double, align 8
  %157 = alloca double, align 8
  %158 = alloca double, align 8
  %159 = alloca %"class.Eigen::Product.154", align 8
  %160 = alloca %class.Vector3_t, align 8
  %161 = alloca double, align 8
  %162 = alloca double, align 8
  %163 = alloca double, align 8
  %164 = alloca double, align 8
  %165 = alloca double, align 8
  %166 = alloca double, align 8
  %167 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %168 = alloca double, align 8
  %169 = alloca double, align 8
  %170 = alloca double, align 8
  %171 = alloca double, align 8
  %172 = alloca double, align 8
  %173 = alloca double, align 8
  %174 = alloca double, align 8
  %175 = alloca double, align 8
  %176 = alloca double, align 8
  %177 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %178 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %179 = alloca %class.Matrix3_t, align 8
  %180 = alloca double, align 8
  %181 = alloca double, align 8
  %182 = alloca double, align 8
  %183 = alloca double, align 8
  %184 = alloca double, align 8
  %185 = alloca double, align 8
  %186 = alloca double, align 8
  %187 = alloca double, align 8
  %188 = alloca %class.Vector3_t, align 8
  %189 = alloca %"class.Eigen::CwiseNullaryOp.149", align 8
  %190 = alloca double, align 8
  %191 = alloca double, align 8
  %192 = alloca double, align 8
  %193 = alloca %"class.Eigen::Product.154", align 8
  %194 = alloca %class.Vector3_t, align 8
  %195 = alloca double, align 8
  %196 = alloca double, align 8
  %197 = alloca double, align 8
  %198 = alloca double, align 8
  %199 = alloca double, align 8
  %200 = alloca double, align 8
  %201 = alloca double, align 8
  %202 = alloca double, align 8
  %203 = alloca double, align 8
  %204 = alloca double, align 8
  %205 = alloca double, align 8
  %206 = alloca double, align 8
  %207 = alloca %"class.Eigen::Product.154", align 8
  %208 = alloca %class.Vector3_t, align 8
  %209 = alloca double, align 8
  %210 = alloca double, align 8
  %211 = alloca double, align 8
  %212 = alloca double, align 8
  %213 = alloca double, align 8
  %214 = alloca double, align 8
  %215 = alloca %"class.Eigen::CwiseBinaryOp.161", align 8
  %216 = alloca %"class.Eigen::Product.167", align 8
  %217 = alloca %"class.Eigen::Transpose", align 8
  %218 = alloca %class.Vector3_t, align 8
  %219 = alloca ptr, align 8
  %220 = alloca ptr, align 8
  store ptr %0, ptr %5, align 8, !tbaa !5
  store i32 %1, ptr %6, align 4, !tbaa !9
  store ptr %2, ptr %7, align 8, !tbaa !5
  store ptr %3, ptr %8, align 8, !tbaa !5
  %221 = load i32, ptr %6, align 4, !tbaa !9
  %222 = icmp ugt i32 %221, 0
  br i1 %222, label %223, label %224

223:                                              ; preds = %4
  br label %226

224:                                              ; preds = %4
  call void @__assert_fail(ptr noundef @.str, ptr noundef @.str.4, i32 noundef 37, ptr noundef @__PRETTY_FUNCTION__._ZN17RigidBodyDynamics5jcalcERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEES6_) #9
  unreachable

225:                                              ; No predecessors!
  br label %226

226:                                              ; preds = %225, %223
  %227 = load ptr, ptr %5, align 8, !tbaa !5
  %228 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %227, i32 0, i32 10
  %229 = load i32, ptr %6, align 4, !tbaa !9
  %230 = zext i32 %229 to i64
  %231 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %228, i64 noundef %230) #10
  %232 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %231, i32 0, i32 1
  %233 = load i32, ptr %232, align 8, !tbaa !11
  %234 = icmp eq i32 %233, 3
  br i1 %234, label %235, label %428

235:                                              ; preds = %226
  call void @llvm.lifetime.start.p0(i64 8, ptr %9) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %10) #10
  %236 = load ptr, ptr %7, align 8, !tbaa !5
  %237 = load ptr, ptr %5, align 8, !tbaa !5
  %238 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %237, i32 0, i32 10
  %239 = load i32, ptr %6, align 4, !tbaa !9
  %240 = zext i32 %239 to i64
  %241 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %238, i64 noundef %240) #10
  %242 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %241, i32 0, i32 3
  %243 = load i32, ptr %242, align 8, !tbaa !14
  %244 = zext i32 %243 to i64
  %245 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %236, i64 noundef %244)
  %246 = load double, ptr %245, align 8, !tbaa !15
  call void @sincos(double noundef %246, ptr noundef %9, ptr noundef %10) #10
  call void @llvm.lifetime.start.p0(i64 72, ptr %11) #10
  %247 = load ptr, ptr %5, align 8, !tbaa !5
  %248 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %247, i32 0, i32 15
  %249 = load i32, ptr %6, align 4, !tbaa !9
  %250 = zext i32 %249 to i64
  %251 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %248, i64 noundef %250) #10
  %252 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %251, i32 0, i32 0
  %253 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %252, i64 noundef 0, i64 noundef 0)
  %254 = load ptr, ptr %5, align 8, !tbaa !5
  %255 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %254, i32 0, i32 15
  %256 = load i32, ptr %6, align 4, !tbaa !9
  %257 = zext i32 %256 to i64
  %258 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %255, i64 noundef %257) #10
  %259 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %258, i32 0, i32 0
  %260 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %259, i64 noundef 0, i64 noundef 1)
  %261 = load ptr, ptr %5, align 8, !tbaa !5
  %262 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %261, i32 0, i32 15
  %263 = load i32, ptr %6, align 4, !tbaa !9
  %264 = zext i32 %263 to i64
  %265 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %262, i64 noundef %264) #10
  %266 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %265, i32 0, i32 0
  %267 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %266, i64 noundef 0, i64 noundef 2)
  call void @llvm.lifetime.start.p0(i64 8, ptr %12) #10
  %268 = load double, ptr %10, align 8, !tbaa !15
  %269 = load ptr, ptr %5, align 8, !tbaa !5
  %270 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %269, i32 0, i32 15
  %271 = load i32, ptr %6, align 4, !tbaa !9
  %272 = zext i32 %271 to i64
  %273 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %270, i64 noundef %272) #10
  %274 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %273, i32 0, i32 0
  %275 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %274, i64 noundef 1, i64 noundef 0)
  %276 = load double, ptr %275, align 8, !tbaa !15
  %277 = load double, ptr %9, align 8, !tbaa !15
  %278 = load ptr, ptr %5, align 8, !tbaa !5
  %279 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %278, i32 0, i32 15
  %280 = load i32, ptr %6, align 4, !tbaa !9
  %281 = zext i32 %280 to i64
  %282 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %279, i64 noundef %281) #10
  %283 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %282, i32 0, i32 0
  %284 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %283, i64 noundef 2, i64 noundef 0)
  %285 = load double, ptr %284, align 8, !tbaa !15
  %286 = fmul double %277, %285
  %287 = call double @llvm.fmuladd.f64(double %268, double %276, double %286)
  store double %287, ptr %12, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %13) #10
  %288 = load double, ptr %10, align 8, !tbaa !15
  %289 = load ptr, ptr %5, align 8, !tbaa !5
  %290 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %289, i32 0, i32 15
  %291 = load i32, ptr %6, align 4, !tbaa !9
  %292 = zext i32 %291 to i64
  %293 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %290, i64 noundef %292) #10
  %294 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %293, i32 0, i32 0
  %295 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %294, i64 noundef 1, i64 noundef 1)
  %296 = load double, ptr %295, align 8, !tbaa !15
  %297 = load double, ptr %9, align 8, !tbaa !15
  %298 = load ptr, ptr %5, align 8, !tbaa !5
  %299 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %298, i32 0, i32 15
  %300 = load i32, ptr %6, align 4, !tbaa !9
  %301 = zext i32 %300 to i64
  %302 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %299, i64 noundef %301) #10
  %303 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %302, i32 0, i32 0
  %304 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %303, i64 noundef 2, i64 noundef 1)
  %305 = load double, ptr %304, align 8, !tbaa !15
  %306 = fmul double %297, %305
  %307 = call double @llvm.fmuladd.f64(double %288, double %296, double %306)
  store double %307, ptr %13, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %14) #10
  %308 = load double, ptr %10, align 8, !tbaa !15
  %309 = load ptr, ptr %5, align 8, !tbaa !5
  %310 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %309, i32 0, i32 15
  %311 = load i32, ptr %6, align 4, !tbaa !9
  %312 = zext i32 %311 to i64
  %313 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %310, i64 noundef %312) #10
  %314 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %313, i32 0, i32 0
  %315 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %314, i64 noundef 1, i64 noundef 2)
  %316 = load double, ptr %315, align 8, !tbaa !15
  %317 = load double, ptr %9, align 8, !tbaa !15
  %318 = load ptr, ptr %5, align 8, !tbaa !5
  %319 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %318, i32 0, i32 15
  %320 = load i32, ptr %6, align 4, !tbaa !9
  %321 = zext i32 %320 to i64
  %322 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %319, i64 noundef %321) #10
  %323 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %322, i32 0, i32 0
  %324 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %323, i64 noundef 2, i64 noundef 2)
  %325 = load double, ptr %324, align 8, !tbaa !15
  %326 = fmul double %317, %325
  %327 = call double @llvm.fmuladd.f64(double %308, double %316, double %326)
  store double %327, ptr %14, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %15) #10
  %328 = load double, ptr %9, align 8, !tbaa !15
  %329 = fneg double %328
  %330 = load ptr, ptr %5, align 8, !tbaa !5
  %331 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %330, i32 0, i32 15
  %332 = load i32, ptr %6, align 4, !tbaa !9
  %333 = zext i32 %332 to i64
  %334 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %331, i64 noundef %333) #10
  %335 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %334, i32 0, i32 0
  %336 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %335, i64 noundef 1, i64 noundef 0)
  %337 = load double, ptr %336, align 8, !tbaa !15
  %338 = load double, ptr %10, align 8, !tbaa !15
  %339 = load ptr, ptr %5, align 8, !tbaa !5
  %340 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %339, i32 0, i32 15
  %341 = load i32, ptr %6, align 4, !tbaa !9
  %342 = zext i32 %341 to i64
  %343 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %340, i64 noundef %342) #10
  %344 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %343, i32 0, i32 0
  %345 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %344, i64 noundef 2, i64 noundef 0)
  %346 = load double, ptr %345, align 8, !tbaa !15
  %347 = fmul double %338, %346
  %348 = call double @llvm.fmuladd.f64(double %329, double %337, double %347)
  store double %348, ptr %15, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %16) #10
  %349 = load double, ptr %9, align 8, !tbaa !15
  %350 = fneg double %349
  %351 = load ptr, ptr %5, align 8, !tbaa !5
  %352 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %351, i32 0, i32 15
  %353 = load i32, ptr %6, align 4, !tbaa !9
  %354 = zext i32 %353 to i64
  %355 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %352, i64 noundef %354) #10
  %356 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %355, i32 0, i32 0
  %357 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %356, i64 noundef 1, i64 noundef 1)
  %358 = load double, ptr %357, align 8, !tbaa !15
  %359 = load double, ptr %10, align 8, !tbaa !15
  %360 = load ptr, ptr %5, align 8, !tbaa !5
  %361 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %360, i32 0, i32 15
  %362 = load i32, ptr %6, align 4, !tbaa !9
  %363 = zext i32 %362 to i64
  %364 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %361, i64 noundef %363) #10
  %365 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %364, i32 0, i32 0
  %366 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %365, i64 noundef 2, i64 noundef 1)
  %367 = load double, ptr %366, align 8, !tbaa !15
  %368 = fmul double %359, %367
  %369 = call double @llvm.fmuladd.f64(double %350, double %358, double %368)
  store double %369, ptr %16, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %17) #10
  %370 = load double, ptr %9, align 8, !tbaa !15
  %371 = fneg double %370
  %372 = load ptr, ptr %5, align 8, !tbaa !5
  %373 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %372, i32 0, i32 15
  %374 = load i32, ptr %6, align 4, !tbaa !9
  %375 = zext i32 %374 to i64
  %376 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %373, i64 noundef %375) #10
  %377 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %376, i32 0, i32 0
  %378 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %377, i64 noundef 1, i64 noundef 2)
  %379 = load double, ptr %378, align 8, !tbaa !15
  %380 = load double, ptr %10, align 8, !tbaa !15
  %381 = load ptr, ptr %5, align 8, !tbaa !5
  %382 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %381, i32 0, i32 15
  %383 = load i32, ptr %6, align 4, !tbaa !9
  %384 = zext i32 %383 to i64
  %385 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %382, i64 noundef %384) #10
  %386 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %385, i32 0, i32 0
  %387 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %386, i64 noundef 2, i64 noundef 2)
  %388 = load double, ptr %387, align 8, !tbaa !15
  %389 = fmul double %380, %388
  %390 = call double @llvm.fmuladd.f64(double %371, double %379, double %389)
  store double %390, ptr %17, align 8, !tbaa !15
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %11, ptr noundef nonnull align 8 dereferenceable(8) %253, ptr noundef nonnull align 8 dereferenceable(8) %260, ptr noundef nonnull align 8 dereferenceable(8) %267, ptr noundef nonnull align 8 dereferenceable(8) %12, ptr noundef nonnull align 8 dereferenceable(8) %13, ptr noundef nonnull align 8 dereferenceable(8) %14, ptr noundef nonnull align 8 dereferenceable(8) %15, ptr noundef nonnull align 8 dereferenceable(8) %16, ptr noundef nonnull align 8 dereferenceable(8) %17)
  %391 = load ptr, ptr %5, align 8, !tbaa !5
  %392 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %391, i32 0, i32 34
  %393 = load i32, ptr %6, align 4, !tbaa !9
  %394 = zext i32 %393 to i64
  %395 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %392, i64 noundef %394) #10
  %396 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %395, i32 0, i32 0
  %397 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(72) %396, ptr noundef nonnull align 8 dereferenceable(72) %11) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %17) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %16) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %15) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %14) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %13) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %12) #10
  call void @llvm.lifetime.end.p0(i64 72, ptr %11) #10
  %398 = load ptr, ptr %5, align 8, !tbaa !5
  %399 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %398, i32 0, i32 15
  %400 = load i32, ptr %6, align 4, !tbaa !9
  %401 = zext i32 %400 to i64
  %402 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %399, i64 noundef %401) #10
  %403 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %402, i32 0, i32 1
  %404 = load ptr, ptr %5, align 8, !tbaa !5
  %405 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %404, i32 0, i32 34
  %406 = load i32, ptr %6, align 4, !tbaa !9
  %407 = zext i32 %406 to i64
  %408 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %405, i64 noundef %407) #10
  %409 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %408, i32 0, i32 1
  %410 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSERKS_(ptr noundef nonnull align 8 dereferenceable(24) %409, ptr noundef nonnull align 8 dereferenceable(24) %403)
  %411 = load ptr, ptr %8, align 8, !tbaa !5
  %412 = load ptr, ptr %5, align 8, !tbaa !5
  %413 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %412, i32 0, i32 10
  %414 = load i32, ptr %6, align 4, !tbaa !9
  %415 = zext i32 %414 to i64
  %416 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %413, i64 noundef %415) #10
  %417 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %416, i32 0, i32 3
  %418 = load i32, ptr %417, align 8, !tbaa !14
  %419 = zext i32 %418 to i64
  %420 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %411, i64 noundef %419)
  %421 = load double, ptr %420, align 8, !tbaa !15
  %422 = load ptr, ptr %5, align 8, !tbaa !5
  %423 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %422, i32 0, i32 12
  %424 = load i32, ptr %6, align 4, !tbaa !9
  %425 = zext i32 %424 to i64
  %426 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %423, i64 noundef %425) #10
  %427 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %426, i64 noundef 0)
  store double %421, ptr %427, align 8, !tbaa !15
  call void @llvm.lifetime.end.p0(i64 8, ptr %10) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %9) #10
  br label %2323

428:                                              ; preds = %226
  %429 = load ptr, ptr %5, align 8, !tbaa !5
  %430 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %429, i32 0, i32 10
  %431 = load i32, ptr %6, align 4, !tbaa !9
  %432 = zext i32 %431 to i64
  %433 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %430, i64 noundef %432) #10
  %434 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %433, i32 0, i32 1
  %435 = load i32, ptr %434, align 8, !tbaa !11
  %436 = icmp eq i32 %435, 4
  br i1 %436, label %437, label %630

437:                                              ; preds = %428
  call void @llvm.lifetime.start.p0(i64 8, ptr %18) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %19) #10
  %438 = load ptr, ptr %7, align 8, !tbaa !5
  %439 = load ptr, ptr %5, align 8, !tbaa !5
  %440 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %439, i32 0, i32 10
  %441 = load i32, ptr %6, align 4, !tbaa !9
  %442 = zext i32 %441 to i64
  %443 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %440, i64 noundef %442) #10
  %444 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %443, i32 0, i32 3
  %445 = load i32, ptr %444, align 8, !tbaa !14
  %446 = zext i32 %445 to i64
  %447 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %438, i64 noundef %446)
  %448 = load double, ptr %447, align 8, !tbaa !15
  call void @sincos(double noundef %448, ptr noundef %18, ptr noundef %19) #10
  call void @llvm.lifetime.start.p0(i64 72, ptr %20) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %21) #10
  %449 = load double, ptr %19, align 8, !tbaa !15
  %450 = load ptr, ptr %5, align 8, !tbaa !5
  %451 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %450, i32 0, i32 15
  %452 = load i32, ptr %6, align 4, !tbaa !9
  %453 = zext i32 %452 to i64
  %454 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %451, i64 noundef %453) #10
  %455 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %454, i32 0, i32 0
  %456 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %455, i64 noundef 0, i64 noundef 0)
  %457 = load double, ptr %456, align 8, !tbaa !15
  %458 = load double, ptr %18, align 8, !tbaa !15
  %459 = fneg double %458
  %460 = load ptr, ptr %5, align 8, !tbaa !5
  %461 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %460, i32 0, i32 15
  %462 = load i32, ptr %6, align 4, !tbaa !9
  %463 = zext i32 %462 to i64
  %464 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %461, i64 noundef %463) #10
  %465 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %464, i32 0, i32 0
  %466 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %465, i64 noundef 2, i64 noundef 0)
  %467 = load double, ptr %466, align 8, !tbaa !15
  %468 = fmul double %459, %467
  %469 = call double @llvm.fmuladd.f64(double %449, double %457, double %468)
  store double %469, ptr %21, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %22) #10
  %470 = load double, ptr %19, align 8, !tbaa !15
  %471 = load ptr, ptr %5, align 8, !tbaa !5
  %472 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %471, i32 0, i32 15
  %473 = load i32, ptr %6, align 4, !tbaa !9
  %474 = zext i32 %473 to i64
  %475 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %472, i64 noundef %474) #10
  %476 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %475, i32 0, i32 0
  %477 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %476, i64 noundef 0, i64 noundef 1)
  %478 = load double, ptr %477, align 8, !tbaa !15
  %479 = load double, ptr %18, align 8, !tbaa !15
  %480 = fneg double %479
  %481 = load ptr, ptr %5, align 8, !tbaa !5
  %482 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %481, i32 0, i32 15
  %483 = load i32, ptr %6, align 4, !tbaa !9
  %484 = zext i32 %483 to i64
  %485 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %482, i64 noundef %484) #10
  %486 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %485, i32 0, i32 0
  %487 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %486, i64 noundef 2, i64 noundef 1)
  %488 = load double, ptr %487, align 8, !tbaa !15
  %489 = fmul double %480, %488
  %490 = call double @llvm.fmuladd.f64(double %470, double %478, double %489)
  store double %490, ptr %22, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %23) #10
  %491 = load double, ptr %19, align 8, !tbaa !15
  %492 = load ptr, ptr %5, align 8, !tbaa !5
  %493 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %492, i32 0, i32 15
  %494 = load i32, ptr %6, align 4, !tbaa !9
  %495 = zext i32 %494 to i64
  %496 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %493, i64 noundef %495) #10
  %497 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %496, i32 0, i32 0
  %498 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %497, i64 noundef 0, i64 noundef 2)
  %499 = load double, ptr %498, align 8, !tbaa !15
  %500 = load double, ptr %18, align 8, !tbaa !15
  %501 = fneg double %500
  %502 = load ptr, ptr %5, align 8, !tbaa !5
  %503 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %502, i32 0, i32 15
  %504 = load i32, ptr %6, align 4, !tbaa !9
  %505 = zext i32 %504 to i64
  %506 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %503, i64 noundef %505) #10
  %507 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %506, i32 0, i32 0
  %508 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %507, i64 noundef 2, i64 noundef 2)
  %509 = load double, ptr %508, align 8, !tbaa !15
  %510 = fmul double %501, %509
  %511 = call double @llvm.fmuladd.f64(double %491, double %499, double %510)
  store double %511, ptr %23, align 8, !tbaa !15
  %512 = load ptr, ptr %5, align 8, !tbaa !5
  %513 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %512, i32 0, i32 15
  %514 = load i32, ptr %6, align 4, !tbaa !9
  %515 = zext i32 %514 to i64
  %516 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %513, i64 noundef %515) #10
  %517 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %516, i32 0, i32 0
  %518 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %517, i64 noundef 1, i64 noundef 0)
  %519 = load ptr, ptr %5, align 8, !tbaa !5
  %520 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %519, i32 0, i32 15
  %521 = load i32, ptr %6, align 4, !tbaa !9
  %522 = zext i32 %521 to i64
  %523 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %520, i64 noundef %522) #10
  %524 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %523, i32 0, i32 0
  %525 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %524, i64 noundef 1, i64 noundef 1)
  %526 = load ptr, ptr %5, align 8, !tbaa !5
  %527 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %526, i32 0, i32 15
  %528 = load i32, ptr %6, align 4, !tbaa !9
  %529 = zext i32 %528 to i64
  %530 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %527, i64 noundef %529) #10
  %531 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %530, i32 0, i32 0
  %532 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %531, i64 noundef 1, i64 noundef 2)
  call void @llvm.lifetime.start.p0(i64 8, ptr %24) #10
  %533 = load double, ptr %18, align 8, !tbaa !15
  %534 = load ptr, ptr %5, align 8, !tbaa !5
  %535 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %534, i32 0, i32 15
  %536 = load i32, ptr %6, align 4, !tbaa !9
  %537 = zext i32 %536 to i64
  %538 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %535, i64 noundef %537) #10
  %539 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %538, i32 0, i32 0
  %540 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %539, i64 noundef 0, i64 noundef 0)
  %541 = load double, ptr %540, align 8, !tbaa !15
  %542 = load double, ptr %19, align 8, !tbaa !15
  %543 = load ptr, ptr %5, align 8, !tbaa !5
  %544 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %543, i32 0, i32 15
  %545 = load i32, ptr %6, align 4, !tbaa !9
  %546 = zext i32 %545 to i64
  %547 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %544, i64 noundef %546) #10
  %548 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %547, i32 0, i32 0
  %549 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %548, i64 noundef 2, i64 noundef 0)
  %550 = load double, ptr %549, align 8, !tbaa !15
  %551 = fmul double %542, %550
  %552 = call double @llvm.fmuladd.f64(double %533, double %541, double %551)
  store double %552, ptr %24, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %25) #10
  %553 = load double, ptr %18, align 8, !tbaa !15
  %554 = load ptr, ptr %5, align 8, !tbaa !5
  %555 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %554, i32 0, i32 15
  %556 = load i32, ptr %6, align 4, !tbaa !9
  %557 = zext i32 %556 to i64
  %558 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %555, i64 noundef %557) #10
  %559 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %558, i32 0, i32 0
  %560 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %559, i64 noundef 0, i64 noundef 1)
  %561 = load double, ptr %560, align 8, !tbaa !15
  %562 = load double, ptr %19, align 8, !tbaa !15
  %563 = load ptr, ptr %5, align 8, !tbaa !5
  %564 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %563, i32 0, i32 15
  %565 = load i32, ptr %6, align 4, !tbaa !9
  %566 = zext i32 %565 to i64
  %567 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %564, i64 noundef %566) #10
  %568 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %567, i32 0, i32 0
  %569 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %568, i64 noundef 2, i64 noundef 1)
  %570 = load double, ptr %569, align 8, !tbaa !15
  %571 = fmul double %562, %570
  %572 = call double @llvm.fmuladd.f64(double %553, double %561, double %571)
  store double %572, ptr %25, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %26) #10
  %573 = load double, ptr %18, align 8, !tbaa !15
  %574 = load ptr, ptr %5, align 8, !tbaa !5
  %575 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %574, i32 0, i32 15
  %576 = load i32, ptr %6, align 4, !tbaa !9
  %577 = zext i32 %576 to i64
  %578 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %575, i64 noundef %577) #10
  %579 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %578, i32 0, i32 0
  %580 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %579, i64 noundef 0, i64 noundef 2)
  %581 = load double, ptr %580, align 8, !tbaa !15
  %582 = load double, ptr %19, align 8, !tbaa !15
  %583 = load ptr, ptr %5, align 8, !tbaa !5
  %584 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %583, i32 0, i32 15
  %585 = load i32, ptr %6, align 4, !tbaa !9
  %586 = zext i32 %585 to i64
  %587 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %584, i64 noundef %586) #10
  %588 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %587, i32 0, i32 0
  %589 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %588, i64 noundef 2, i64 noundef 2)
  %590 = load double, ptr %589, align 8, !tbaa !15
  %591 = fmul double %582, %590
  %592 = call double @llvm.fmuladd.f64(double %573, double %581, double %591)
  store double %592, ptr %26, align 8, !tbaa !15
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %20, ptr noundef nonnull align 8 dereferenceable(8) %21, ptr noundef nonnull align 8 dereferenceable(8) %22, ptr noundef nonnull align 8 dereferenceable(8) %23, ptr noundef nonnull align 8 dereferenceable(8) %518, ptr noundef nonnull align 8 dereferenceable(8) %525, ptr noundef nonnull align 8 dereferenceable(8) %532, ptr noundef nonnull align 8 dereferenceable(8) %24, ptr noundef nonnull align 8 dereferenceable(8) %25, ptr noundef nonnull align 8 dereferenceable(8) %26)
  %593 = load ptr, ptr %5, align 8, !tbaa !5
  %594 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %593, i32 0, i32 34
  %595 = load i32, ptr %6, align 4, !tbaa !9
  %596 = zext i32 %595 to i64
  %597 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %594, i64 noundef %596) #10
  %598 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %597, i32 0, i32 0
  %599 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(72) %598, ptr noundef nonnull align 8 dereferenceable(72) %20) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %26) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %25) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %24) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %23) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %22) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %21) #10
  call void @llvm.lifetime.end.p0(i64 72, ptr %20) #10
  %600 = load ptr, ptr %5, align 8, !tbaa !5
  %601 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %600, i32 0, i32 15
  %602 = load i32, ptr %6, align 4, !tbaa !9
  %603 = zext i32 %602 to i64
  %604 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %601, i64 noundef %603) #10
  %605 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %604, i32 0, i32 1
  %606 = load ptr, ptr %5, align 8, !tbaa !5
  %607 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %606, i32 0, i32 34
  %608 = load i32, ptr %6, align 4, !tbaa !9
  %609 = zext i32 %608 to i64
  %610 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %607, i64 noundef %609) #10
  %611 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %610, i32 0, i32 1
  %612 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSERKS_(ptr noundef nonnull align 8 dereferenceable(24) %611, ptr noundef nonnull align 8 dereferenceable(24) %605)
  %613 = load ptr, ptr %8, align 8, !tbaa !5
  %614 = load ptr, ptr %5, align 8, !tbaa !5
  %615 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %614, i32 0, i32 10
  %616 = load i32, ptr %6, align 4, !tbaa !9
  %617 = zext i32 %616 to i64
  %618 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %615, i64 noundef %617) #10
  %619 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %618, i32 0, i32 3
  %620 = load i32, ptr %619, align 8, !tbaa !14
  %621 = zext i32 %620 to i64
  %622 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %613, i64 noundef %621)
  %623 = load double, ptr %622, align 8, !tbaa !15
  %624 = load ptr, ptr %5, align 8, !tbaa !5
  %625 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %624, i32 0, i32 12
  %626 = load i32, ptr %6, align 4, !tbaa !9
  %627 = zext i32 %626 to i64
  %628 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %625, i64 noundef %627) #10
  %629 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %628, i64 noundef 1)
  store double %623, ptr %629, align 8, !tbaa !15
  call void @llvm.lifetime.end.p0(i64 8, ptr %19) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %18) #10
  br label %2322

630:                                              ; preds = %428
  %631 = load ptr, ptr %5, align 8, !tbaa !5
  %632 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %631, i32 0, i32 10
  %633 = load i32, ptr %6, align 4, !tbaa !9
  %634 = zext i32 %633 to i64
  %635 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %632, i64 noundef %634) #10
  %636 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %635, i32 0, i32 1
  %637 = load i32, ptr %636, align 8, !tbaa !11
  %638 = icmp eq i32 %637, 5
  br i1 %638, label %639, label %832

639:                                              ; preds = %630
  call void @llvm.lifetime.start.p0(i64 8, ptr %27) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %28) #10
  %640 = load ptr, ptr %7, align 8, !tbaa !5
  %641 = load ptr, ptr %5, align 8, !tbaa !5
  %642 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %641, i32 0, i32 10
  %643 = load i32, ptr %6, align 4, !tbaa !9
  %644 = zext i32 %643 to i64
  %645 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %642, i64 noundef %644) #10
  %646 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %645, i32 0, i32 3
  %647 = load i32, ptr %646, align 8, !tbaa !14
  %648 = zext i32 %647 to i64
  %649 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %640, i64 noundef %648)
  %650 = load double, ptr %649, align 8, !tbaa !15
  call void @sincos(double noundef %650, ptr noundef %27, ptr noundef %28) #10
  call void @llvm.lifetime.start.p0(i64 72, ptr %29) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %30) #10
  %651 = load double, ptr %28, align 8, !tbaa !15
  %652 = load ptr, ptr %5, align 8, !tbaa !5
  %653 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %652, i32 0, i32 15
  %654 = load i32, ptr %6, align 4, !tbaa !9
  %655 = zext i32 %654 to i64
  %656 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %653, i64 noundef %655) #10
  %657 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %656, i32 0, i32 0
  %658 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %657, i64 noundef 0, i64 noundef 0)
  %659 = load double, ptr %658, align 8, !tbaa !15
  %660 = load double, ptr %27, align 8, !tbaa !15
  %661 = load ptr, ptr %5, align 8, !tbaa !5
  %662 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %661, i32 0, i32 15
  %663 = load i32, ptr %6, align 4, !tbaa !9
  %664 = zext i32 %663 to i64
  %665 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %662, i64 noundef %664) #10
  %666 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %665, i32 0, i32 0
  %667 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %666, i64 noundef 1, i64 noundef 0)
  %668 = load double, ptr %667, align 8, !tbaa !15
  %669 = fmul double %660, %668
  %670 = call double @llvm.fmuladd.f64(double %651, double %659, double %669)
  store double %670, ptr %30, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %31) #10
  %671 = load double, ptr %28, align 8, !tbaa !15
  %672 = load ptr, ptr %5, align 8, !tbaa !5
  %673 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %672, i32 0, i32 15
  %674 = load i32, ptr %6, align 4, !tbaa !9
  %675 = zext i32 %674 to i64
  %676 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %673, i64 noundef %675) #10
  %677 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %676, i32 0, i32 0
  %678 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %677, i64 noundef 0, i64 noundef 1)
  %679 = load double, ptr %678, align 8, !tbaa !15
  %680 = load double, ptr %27, align 8, !tbaa !15
  %681 = load ptr, ptr %5, align 8, !tbaa !5
  %682 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %681, i32 0, i32 15
  %683 = load i32, ptr %6, align 4, !tbaa !9
  %684 = zext i32 %683 to i64
  %685 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %682, i64 noundef %684) #10
  %686 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %685, i32 0, i32 0
  %687 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %686, i64 noundef 1, i64 noundef 1)
  %688 = load double, ptr %687, align 8, !tbaa !15
  %689 = fmul double %680, %688
  %690 = call double @llvm.fmuladd.f64(double %671, double %679, double %689)
  store double %690, ptr %31, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %32) #10
  %691 = load double, ptr %28, align 8, !tbaa !15
  %692 = load ptr, ptr %5, align 8, !tbaa !5
  %693 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %692, i32 0, i32 15
  %694 = load i32, ptr %6, align 4, !tbaa !9
  %695 = zext i32 %694 to i64
  %696 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %693, i64 noundef %695) #10
  %697 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %696, i32 0, i32 0
  %698 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %697, i64 noundef 0, i64 noundef 2)
  %699 = load double, ptr %698, align 8, !tbaa !15
  %700 = load double, ptr %27, align 8, !tbaa !15
  %701 = load ptr, ptr %5, align 8, !tbaa !5
  %702 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %701, i32 0, i32 15
  %703 = load i32, ptr %6, align 4, !tbaa !9
  %704 = zext i32 %703 to i64
  %705 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %702, i64 noundef %704) #10
  %706 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %705, i32 0, i32 0
  %707 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %706, i64 noundef 1, i64 noundef 2)
  %708 = load double, ptr %707, align 8, !tbaa !15
  %709 = fmul double %700, %708
  %710 = call double @llvm.fmuladd.f64(double %691, double %699, double %709)
  store double %710, ptr %32, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %33) #10
  %711 = load double, ptr %27, align 8, !tbaa !15
  %712 = fneg double %711
  %713 = load ptr, ptr %5, align 8, !tbaa !5
  %714 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %713, i32 0, i32 15
  %715 = load i32, ptr %6, align 4, !tbaa !9
  %716 = zext i32 %715 to i64
  %717 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %714, i64 noundef %716) #10
  %718 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %717, i32 0, i32 0
  %719 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %718, i64 noundef 0, i64 noundef 0)
  %720 = load double, ptr %719, align 8, !tbaa !15
  %721 = load double, ptr %28, align 8, !tbaa !15
  %722 = load ptr, ptr %5, align 8, !tbaa !5
  %723 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %722, i32 0, i32 15
  %724 = load i32, ptr %6, align 4, !tbaa !9
  %725 = zext i32 %724 to i64
  %726 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %723, i64 noundef %725) #10
  %727 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %726, i32 0, i32 0
  %728 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %727, i64 noundef 1, i64 noundef 0)
  %729 = load double, ptr %728, align 8, !tbaa !15
  %730 = fmul double %721, %729
  %731 = call double @llvm.fmuladd.f64(double %712, double %720, double %730)
  store double %731, ptr %33, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %34) #10
  %732 = load double, ptr %27, align 8, !tbaa !15
  %733 = fneg double %732
  %734 = load ptr, ptr %5, align 8, !tbaa !5
  %735 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %734, i32 0, i32 15
  %736 = load i32, ptr %6, align 4, !tbaa !9
  %737 = zext i32 %736 to i64
  %738 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %735, i64 noundef %737) #10
  %739 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %738, i32 0, i32 0
  %740 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %739, i64 noundef 0, i64 noundef 1)
  %741 = load double, ptr %740, align 8, !tbaa !15
  %742 = load double, ptr %28, align 8, !tbaa !15
  %743 = load ptr, ptr %5, align 8, !tbaa !5
  %744 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %743, i32 0, i32 15
  %745 = load i32, ptr %6, align 4, !tbaa !9
  %746 = zext i32 %745 to i64
  %747 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %744, i64 noundef %746) #10
  %748 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %747, i32 0, i32 0
  %749 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %748, i64 noundef 1, i64 noundef 1)
  %750 = load double, ptr %749, align 8, !tbaa !15
  %751 = fmul double %742, %750
  %752 = call double @llvm.fmuladd.f64(double %733, double %741, double %751)
  store double %752, ptr %34, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %35) #10
  %753 = load double, ptr %27, align 8, !tbaa !15
  %754 = fneg double %753
  %755 = load ptr, ptr %5, align 8, !tbaa !5
  %756 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %755, i32 0, i32 15
  %757 = load i32, ptr %6, align 4, !tbaa !9
  %758 = zext i32 %757 to i64
  %759 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %756, i64 noundef %758) #10
  %760 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %759, i32 0, i32 0
  %761 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %760, i64 noundef 0, i64 noundef 2)
  %762 = load double, ptr %761, align 8, !tbaa !15
  %763 = load double, ptr %28, align 8, !tbaa !15
  %764 = load ptr, ptr %5, align 8, !tbaa !5
  %765 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %764, i32 0, i32 15
  %766 = load i32, ptr %6, align 4, !tbaa !9
  %767 = zext i32 %766 to i64
  %768 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %765, i64 noundef %767) #10
  %769 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %768, i32 0, i32 0
  %770 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %769, i64 noundef 1, i64 noundef 2)
  %771 = load double, ptr %770, align 8, !tbaa !15
  %772 = fmul double %763, %771
  %773 = call double @llvm.fmuladd.f64(double %754, double %762, double %772)
  store double %773, ptr %35, align 8, !tbaa !15
  %774 = load ptr, ptr %5, align 8, !tbaa !5
  %775 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %774, i32 0, i32 15
  %776 = load i32, ptr %6, align 4, !tbaa !9
  %777 = zext i32 %776 to i64
  %778 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %775, i64 noundef %777) #10
  %779 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %778, i32 0, i32 0
  %780 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %779, i64 noundef 2, i64 noundef 0)
  %781 = load ptr, ptr %5, align 8, !tbaa !5
  %782 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %781, i32 0, i32 15
  %783 = load i32, ptr %6, align 4, !tbaa !9
  %784 = zext i32 %783 to i64
  %785 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %782, i64 noundef %784) #10
  %786 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %785, i32 0, i32 0
  %787 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %786, i64 noundef 2, i64 noundef 1)
  %788 = load ptr, ptr %5, align 8, !tbaa !5
  %789 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %788, i32 0, i32 15
  %790 = load i32, ptr %6, align 4, !tbaa !9
  %791 = zext i32 %790 to i64
  %792 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %789, i64 noundef %791) #10
  %793 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %792, i32 0, i32 0
  %794 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %793, i64 noundef 2, i64 noundef 2)
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %29, ptr noundef nonnull align 8 dereferenceable(8) %30, ptr noundef nonnull align 8 dereferenceable(8) %31, ptr noundef nonnull align 8 dereferenceable(8) %32, ptr noundef nonnull align 8 dereferenceable(8) %33, ptr noundef nonnull align 8 dereferenceable(8) %34, ptr noundef nonnull align 8 dereferenceable(8) %35, ptr noundef nonnull align 8 dereferenceable(8) %780, ptr noundef nonnull align 8 dereferenceable(8) %787, ptr noundef nonnull align 8 dereferenceable(8) %794)
  %795 = load ptr, ptr %5, align 8, !tbaa !5
  %796 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %795, i32 0, i32 34
  %797 = load i32, ptr %6, align 4, !tbaa !9
  %798 = zext i32 %797 to i64
  %799 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %796, i64 noundef %798) #10
  %800 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %799, i32 0, i32 0
  %801 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(72) %800, ptr noundef nonnull align 8 dereferenceable(72) %29) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %35) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %34) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %33) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %32) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %31) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %30) #10
  call void @llvm.lifetime.end.p0(i64 72, ptr %29) #10
  %802 = load ptr, ptr %5, align 8, !tbaa !5
  %803 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %802, i32 0, i32 15
  %804 = load i32, ptr %6, align 4, !tbaa !9
  %805 = zext i32 %804 to i64
  %806 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %803, i64 noundef %805) #10
  %807 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %806, i32 0, i32 1
  %808 = load ptr, ptr %5, align 8, !tbaa !5
  %809 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %808, i32 0, i32 34
  %810 = load i32, ptr %6, align 4, !tbaa !9
  %811 = zext i32 %810 to i64
  %812 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %809, i64 noundef %811) #10
  %813 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %812, i32 0, i32 1
  %814 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSERKS_(ptr noundef nonnull align 8 dereferenceable(24) %813, ptr noundef nonnull align 8 dereferenceable(24) %807)
  %815 = load ptr, ptr %8, align 8, !tbaa !5
  %816 = load ptr, ptr %5, align 8, !tbaa !5
  %817 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %816, i32 0, i32 10
  %818 = load i32, ptr %6, align 4, !tbaa !9
  %819 = zext i32 %818 to i64
  %820 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %817, i64 noundef %819) #10
  %821 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %820, i32 0, i32 3
  %822 = load i32, ptr %821, align 8, !tbaa !14
  %823 = zext i32 %822 to i64
  %824 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %815, i64 noundef %823)
  %825 = load double, ptr %824, align 8, !tbaa !15
  %826 = load ptr, ptr %5, align 8, !tbaa !5
  %827 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %826, i32 0, i32 12
  %828 = load i32, ptr %6, align 4, !tbaa !9
  %829 = zext i32 %828 to i64
  %830 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %827, i64 noundef %829) #10
  %831 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %830, i64 noundef 2)
  store double %825, ptr %831, align 8, !tbaa !15
  call void @llvm.lifetime.end.p0(i64 8, ptr %28) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %27) #10
  br label %2321

832:                                              ; preds = %630
  %833 = load ptr, ptr %5, align 8, !tbaa !5
  %834 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %833, i32 0, i32 10
  %835 = load i32, ptr %6, align 4, !tbaa !9
  %836 = zext i32 %835 to i64
  %837 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %834, i64 noundef %836) #10
  %838 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %837, i32 0, i32 1
  %839 = load i32, ptr %838, align 8, !tbaa !11
  %840 = icmp eq i32 %839, 14
  br i1 %840, label %841, label %910

841:                                              ; preds = %832
  call void @llvm.lifetime.start.p0(i64 96, ptr %36) #10
  %842 = load ptr, ptr %5, align 8, !tbaa !5
  %843 = load i32, ptr %6, align 4, !tbaa !9
  %844 = load ptr, ptr %7, align 8, !tbaa !5
  call void @_ZN17RigidBodyDynamics8jcalc_XJERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %36, ptr noundef nonnull align 8 dereferenceable(872) %842, i32 noundef %843, ptr noundef nonnull align 8 dereferenceable(16) %844)
  %845 = load ptr, ptr %5, align 8, !tbaa !5
  %846 = load i32, ptr %6, align 4, !tbaa !9
  %847 = load ptr, ptr %7, align 8, !tbaa !5
  call void @_ZN17RigidBodyDynamics16jcalc_X_lambda_SERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr noundef nonnull align 8 dereferenceable(872) %845, i32 noundef %846, ptr noundef nonnull align 8 dereferenceable(16) %847)
  call void @llvm.lifetime.start.p0(i64 8, ptr %37) #10
  %848 = load ptr, ptr %8, align 8, !tbaa !5
  %849 = load ptr, ptr %5, align 8, !tbaa !5
  %850 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %849, i32 0, i32 10
  %851 = load i32, ptr %6, align 4, !tbaa !9
  %852 = zext i32 %851 to i64
  %853 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %850, i64 noundef %852) #10
  %854 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %853, i32 0, i32 3
  %855 = load i32, ptr %854, align 8, !tbaa !14
  %856 = zext i32 %855 to i64
  %857 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %848, i64 noundef %856)
  %858 = load double, ptr %857, align 8, !tbaa !15
  store double %858, ptr %37, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 32, ptr %38) #10
  %859 = load ptr, ptr %5, align 8, !tbaa !5
  %860 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %859, i32 0, i32 11
  %861 = load i32, ptr %6, align 4, !tbaa !9
  %862 = zext i32 %861 to i64
  %863 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %860, i64 noundef %862) #10
  call void @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEEmlIdEEKNS_13CwiseBinaryOpINS_8internal17scalar_product_opIdNS6_18promote_scalar_argIdT_Xsr5Eigen8internal14has_ReturnTypeINS_20ScalarBinaryOpTraitsIdS9_NS7_IdS9_EEEEEE5valueEE4typeEEEKS2_KNS6_19plain_constant_typeIS2_SE_E4typeEEERKS9_(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseBinaryOp") align 8 %38, ptr noundef nonnull align 1 dereferenceable(1) %863, ptr noundef nonnull align 8 dereferenceable(8) %37)
  %864 = load ptr, ptr %5, align 8, !tbaa !5
  %865 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %864, i32 0, i32 12
  %866 = load i32, ptr %6, align 4, !tbaa !9
  %867 = zext i32 %866 to i64
  %868 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %865, i64 noundef %867) #10
  %869 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen13CwiseBinaryOpINS1_8internal17scalar_product_opIddEEKNS1_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEKNS1_14CwiseNullaryOpINS3_18scalar_constant_opIdEES8_EEEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48) %868, ptr noundef nonnull align 1 dereferenceable(1) %38)
  call void @llvm.lifetime.end.p0(i64 32, ptr %38) #10
  call void @llvm.lifetime.start.p0(i64 24, ptr %39) #10
  call void @llvm.lifetime.start.p0(i64 56, ptr %40) #10
  %870 = load ptr, ptr %5, align 8, !tbaa !5
  %871 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %870, i32 0, i32 11
  %872 = load i32, ptr %6, align 4, !tbaa !9
  %873 = zext i32 %872 to i64
  %874 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %871, i64 noundef %873) #10
  call void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEE5blockIiiEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind writable sret(%"class.Eigen::Block") align 8 %40, ptr noundef nonnull align 1 dereferenceable(1) %874, i64 noundef 0, i64 noundef 0, i32 noundef 3, i32 noundef 1)
  call void @_ZN9Vector3_tC2IN5Eigen5BlockINS1_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELin1ELin1ELb0EEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %39, ptr noundef nonnull align 1 dereferenceable(1) %40)
  call void @llvm.lifetime.end.p0(i64 56, ptr %40) #10
  call void @llvm.lifetime.start.p0(i64 24, ptr %41) #10
  call void @llvm.lifetime.start.p0(i64 64, ptr %42) #10
  %875 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %36, i32 0, i32 0
  call void @llvm.lifetime.start.p0(i64 56, ptr %43) #10
  %876 = load ptr, ptr %5, align 8, !tbaa !5
  %877 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %876, i32 0, i32 10
  %878 = load i32, ptr %6, align 4, !tbaa !9
  %879 = zext i32 %878 to i64
  %880 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %877, i64 noundef %879) #10
  %881 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %880, i32 0, i32 0
  %882 = load ptr, ptr %881, align 8, !tbaa !17
  %883 = getelementptr inbounds %class.SpatialVector_t, ptr %882, i64 0
  call void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEE5blockIiiEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind writable sret(%"class.Eigen::Block") align 8 %43, ptr noundef nonnull align 1 dereferenceable(1) %883, i64 noundef 3, i64 noundef 0, i32 noundef 3, i32 noundef 1)
  call void @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEmlINS_5BlockINS1_IdLi6ELi1ELi0ELi6ELi1EEELin1ELin1ELb0EEEEEKNS_7ProductIS2_T_Li0EEERKNS0_IS9_EE(ptr dead_on_unwind writable sret(%"class.Eigen::Product") align 8 %42, ptr noundef nonnull align 1 dereferenceable(1) %875, ptr noundef nonnull align 1 dereferenceable(1) %43)
  call void @_ZN9Vector3_tC2IN5Eigen7ProductINS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEENS1_5BlockINS3_IdLi6ELi1ELi0ELi6ELi1EEELin1ELin1ELb0EEELi0EEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %41, ptr noundef nonnull align 1 dereferenceable(1) %42)
  call void @llvm.lifetime.end.p0(i64 56, ptr %43) #10
  call void @llvm.lifetime.end.p0(i64 64, ptr %42) #10
  call void @llvm.lifetime.start.p0(i64 24, ptr %44) #10
  call void @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE5crossIS2_EENS3_25cross_product_return_typeIT_E4typeERKNS0_IS6_EE(ptr dead_on_unwind writable sret(%"class.Eigen::Matrix") align 8 %44, ptr noundef nonnull align 1 dereferenceable(1) %39, ptr noundef nonnull align 1 dereferenceable(1) %41)
  %884 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSIN5Eigen6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %41, ptr noundef nonnull align 1 dereferenceable(1) %44)
  call void @llvm.lifetime.end.p0(i64 24, ptr %44) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %45) #10
  %885 = load double, ptr %37, align 8, !tbaa !15
  %886 = fneg double %885
  %887 = load double, ptr %37, align 8, !tbaa !15
  %888 = fmul double %886, %887
  store double %888, ptr %45, align 8, !tbaa !15
  %889 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEmLERKd(ptr noundef nonnull align 1 dereferenceable(1) %41, ptr noundef nonnull align 8 dereferenceable(8) %45)
  call void @llvm.lifetime.end.p0(i64 8, ptr %45) #10
  call void @llvm.lifetime.start.p0(i64 48, ptr %46) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %47) #10
  store double 0.000000e+00, ptr %47, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %48) #10
  store double 0.000000e+00, ptr %48, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %49) #10
  store double 0.000000e+00, ptr %49, align 8, !tbaa !15
  %890 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %41, i64 noundef 0)
  %891 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %41, i64 noundef 1)
  %892 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %41, i64 noundef 2)
  call void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %46, ptr noundef nonnull align 8 dereferenceable(8) %47, ptr noundef nonnull align 8 dereferenceable(8) %48, ptr noundef nonnull align 8 dereferenceable(8) %49, ptr noundef nonnull align 8 dereferenceable(8) %890, ptr noundef nonnull align 8 dereferenceable(8) %891, ptr noundef nonnull align 8 dereferenceable(8) %892)
  %893 = load ptr, ptr %5, align 8, !tbaa !5
  %894 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %893, i32 0, i32 13
  %895 = load i32, ptr %6, align 4, !tbaa !9
  %896 = zext i32 %895 to i64
  %897 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %894, i64 noundef %896) #10
  %898 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSEOS_(ptr noundef nonnull align 16 dereferenceable(48) %897, ptr noundef nonnull align 16 dereferenceable(48) %46) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %49) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %48) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %47) #10
  call void @llvm.lifetime.end.p0(i64 48, ptr %46) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr %50) #10
  %899 = load ptr, ptr %5, align 8, !tbaa !5
  %900 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %899, i32 0, i32 15
  %901 = load i32, ptr %6, align 4, !tbaa !9
  %902 = zext i32 %901 to i64
  %903 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %900, i64 noundef %902) #10
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %50, ptr noundef nonnull align 8 dereferenceable(96) %36, ptr noundef nonnull align 8 dereferenceable(96) %903)
  %904 = load ptr, ptr %5, align 8, !tbaa !5
  %905 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %904, i32 0, i32 34
  %906 = load i32, ptr %6, align 4, !tbaa !9
  %907 = zext i32 %906 to i64
  %908 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %905, i64 noundef %907) #10
  %909 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %908, ptr noundef nonnull align 8 dereferenceable(96) %50) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr %50) #10
  call void @llvm.lifetime.end.p0(i64 24, ptr %41) #10
  call void @llvm.lifetime.end.p0(i64 24, ptr %39) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %37) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr %36) #10
  br label %2320

910:                                              ; preds = %832
  %911 = load ptr, ptr %5, align 8, !tbaa !5
  %912 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %911, i32 0, i32 10
  %913 = load i32, ptr %6, align 4, !tbaa !9
  %914 = zext i32 %913 to i64
  %915 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %912, i64 noundef %914) #10
  %916 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %915, i32 0, i32 2
  %917 = load i32, ptr %916, align 4, !tbaa !18
  %918 = icmp eq i32 %917, 1
  br i1 %918, label %919, label %964

919:                                              ; preds = %910
  %920 = load ptr, ptr %5, align 8, !tbaa !5
  %921 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %920, i32 0, i32 10
  %922 = load i32, ptr %6, align 4, !tbaa !9
  %923 = zext i32 %922 to i64
  %924 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %921, i64 noundef %923) #10
  %925 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %924, i32 0, i32 1
  %926 = load i32, ptr %925, align 8, !tbaa !11
  %927 = icmp ne i32 %926, 21
  br i1 %927, label %928, label %964

928:                                              ; preds = %919
  call void @llvm.lifetime.start.p0(i64 96, ptr %51) #10
  %929 = load ptr, ptr %5, align 8, !tbaa !5
  %930 = load i32, ptr %6, align 4, !tbaa !9
  %931 = load ptr, ptr %7, align 8, !tbaa !5
  call void @_ZN17RigidBodyDynamics8jcalc_XJERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %51, ptr noundef nonnull align 8 dereferenceable(872) %929, i32 noundef %930, ptr noundef nonnull align 8 dereferenceable(16) %931)
  call void @llvm.lifetime.start.p0(i64 32, ptr %52) #10
  %932 = load ptr, ptr %5, align 8, !tbaa !5
  %933 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %932, i32 0, i32 11
  %934 = load i32, ptr %6, align 4, !tbaa !9
  %935 = zext i32 %934 to i64
  %936 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %933, i64 noundef %935) #10
  %937 = load ptr, ptr %8, align 8, !tbaa !5
  %938 = load ptr, ptr %5, align 8, !tbaa !5
  %939 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %938, i32 0, i32 10
  %940 = load i32, ptr %6, align 4, !tbaa !9
  %941 = zext i32 %940 to i64
  %942 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %939, i64 noundef %941) #10
  %943 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %942, i32 0, i32 3
  %944 = load i32, ptr %943, align 8, !tbaa !14
  %945 = zext i32 %944 to i64
  %946 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %937, i64 noundef %945)
  call void @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEEmlIdEEKNS_13CwiseBinaryOpINS_8internal17scalar_product_opIdNS6_18promote_scalar_argIdT_Xsr5Eigen8internal14has_ReturnTypeINS_20ScalarBinaryOpTraitsIdS9_NS7_IdS9_EEEEEE5valueEE4typeEEEKS2_KNS6_19plain_constant_typeIS2_SE_E4typeEEERKS9_(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseBinaryOp") align 8 %52, ptr noundef nonnull align 1 dereferenceable(1) %936, ptr noundef nonnull align 8 dereferenceable(8) %946)
  %947 = load ptr, ptr %5, align 8, !tbaa !5
  %948 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %947, i32 0, i32 12
  %949 = load i32, ptr %6, align 4, !tbaa !9
  %950 = zext i32 %949 to i64
  %951 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %948, i64 noundef %950) #10
  %952 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen13CwiseBinaryOpINS1_8internal17scalar_product_opIddEEKNS1_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEKNS1_14CwiseNullaryOpINS3_18scalar_constant_opIdEES8_EEEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48) %951, ptr noundef nonnull align 1 dereferenceable(1) %52)
  call void @llvm.lifetime.end.p0(i64 32, ptr %52) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr %53) #10
  %953 = load ptr, ptr %5, align 8, !tbaa !5
  %954 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %953, i32 0, i32 15
  %955 = load i32, ptr %6, align 4, !tbaa !9
  %956 = zext i32 %955 to i64
  %957 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %954, i64 noundef %956) #10
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %53, ptr noundef nonnull align 8 dereferenceable(96) %51, ptr noundef nonnull align 8 dereferenceable(96) %957)
  %958 = load ptr, ptr %5, align 8, !tbaa !5
  %959 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %958, i32 0, i32 34
  %960 = load i32, ptr %6, align 4, !tbaa !9
  %961 = zext i32 %960 to i64
  %962 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %959, i64 noundef %961) #10
  %963 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %962, ptr noundef nonnull align 8 dereferenceable(96) %53) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr %53) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr %51) #10
  br label %2319

964:                                              ; preds = %919, %910
  %965 = load ptr, ptr %5, align 8, !tbaa !5
  %966 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %965, i32 0, i32 10
  %967 = load i32, ptr %6, align 4, !tbaa !9
  %968 = zext i32 %967 to i64
  %969 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %966, i64 noundef %968) #10
  %970 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %969, i32 0, i32 1
  %971 = load i32, ptr %970, align 8, !tbaa !11
  %972 = icmp eq i32 %971, 6
  br i1 %972, label %973, label %1047

973:                                              ; preds = %964
  call void @llvm.lifetime.start.p0(i64 96, ptr %54) #10
  call void @llvm.lifetime.start.p0(i64 72, ptr %55) #10
  call void @llvm.lifetime.start.p0(i64 32, ptr %56) #10
  %974 = load ptr, ptr %5, align 8, !tbaa !5
  %975 = load i32, ptr %6, align 4, !tbaa !9
  %976 = load ptr, ptr %7, align 8, !tbaa !5
  call void @_ZNK17RigidBodyDynamics5Model13GetQuaternionEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr dead_on_unwind writable sret(%"class.RigidBodyDynamics::Math::Quaternion") align 16 %56, ptr noundef nonnull align 8 dereferenceable(872) %974, i32 noundef %975, ptr noundef nonnull align 8 dereferenceable(16) %976)
  call void @_ZNK17RigidBodyDynamics4Math10Quaternion8toMatrixEv(ptr dead_on_unwind writable sret(%class.Matrix3_t) align 8 %55, ptr noundef nonnull align 16 dereferenceable(32) %56)
  call void @llvm.lifetime.start.p0(i64 24, ptr %57) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %58) #10
  store double 0.000000e+00, ptr %58, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %59) #10
  store double 0.000000e+00, ptr %59, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %60) #10
  store double 0.000000e+00, ptr %60, align 8, !tbaa !15
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %57, ptr noundef nonnull align 8 dereferenceable(8) %58, ptr noundef nonnull align 8 dereferenceable(8) %59, ptr noundef nonnull align 8 dereferenceable(8) %60)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %54, ptr noundef nonnull align 8 dereferenceable(72) %55, ptr noundef nonnull align 8 dereferenceable(24) %57)
  call void @llvm.lifetime.end.p0(i64 8, ptr %60) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %59) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %58) #10
  call void @llvm.lifetime.end.p0(i64 24, ptr %57) #10
  call void @llvm.lifetime.end.p0(i64 32, ptr %56) #10
  call void @llvm.lifetime.end.p0(i64 72, ptr %55) #10
  %977 = load ptr, ptr %5, align 8, !tbaa !5
  %978 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %977, i32 0, i32 17
  %979 = load i32, ptr %6, align 4, !tbaa !9
  %980 = zext i32 %979 to i64
  %981 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %978, i64 noundef %980) #10
  %982 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %981, i64 noundef 0, i64 noundef 0)
  store double 1.000000e+00, ptr %982, align 8, !tbaa !15
  %983 = load ptr, ptr %5, align 8, !tbaa !5
  %984 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %983, i32 0, i32 17
  %985 = load i32, ptr %6, align 4, !tbaa !9
  %986 = zext i32 %985 to i64
  %987 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %984, i64 noundef %986) #10
  %988 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %987, i64 noundef 1, i64 noundef 1)
  store double 1.000000e+00, ptr %988, align 8, !tbaa !15
  %989 = load ptr, ptr %5, align 8, !tbaa !5
  %990 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %989, i32 0, i32 17
  %991 = load i32, ptr %6, align 4, !tbaa !9
  %992 = zext i32 %991 to i64
  %993 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %990, i64 noundef %992) #10
  %994 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %993, i64 noundef 2, i64 noundef 2)
  store double 1.000000e+00, ptr %994, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 24, ptr %61) #10
  %995 = load ptr, ptr %8, align 8, !tbaa !5
  %996 = load ptr, ptr %5, align 8, !tbaa !5
  %997 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %996, i32 0, i32 10
  %998 = load i32, ptr %6, align 4, !tbaa !9
  %999 = zext i32 %998 to i64
  %1000 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %997, i64 noundef %999) #10
  %1001 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1000, i32 0, i32 3
  %1002 = load i32, ptr %1001, align 8, !tbaa !14
  %1003 = zext i32 %1002 to i64
  %1004 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %995, i64 noundef %1003)
  %1005 = load ptr, ptr %8, align 8, !tbaa !5
  %1006 = load ptr, ptr %5, align 8, !tbaa !5
  %1007 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1006, i32 0, i32 10
  %1008 = load i32, ptr %6, align 4, !tbaa !9
  %1009 = zext i32 %1008 to i64
  %1010 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1007, i64 noundef %1009) #10
  %1011 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1010, i32 0, i32 3
  %1012 = load i32, ptr %1011, align 8, !tbaa !14
  %1013 = add i32 %1012, 1
  %1014 = zext i32 %1013 to i64
  %1015 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1005, i64 noundef %1014)
  %1016 = load ptr, ptr %8, align 8, !tbaa !5
  %1017 = load ptr, ptr %5, align 8, !tbaa !5
  %1018 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1017, i32 0, i32 10
  %1019 = load i32, ptr %6, align 4, !tbaa !9
  %1020 = zext i32 %1019 to i64
  %1021 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1018, i64 noundef %1020) #10
  %1022 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1021, i32 0, i32 3
  %1023 = load i32, ptr %1022, align 8, !tbaa !14
  %1024 = add i32 %1023, 2
  %1025 = zext i32 %1024 to i64
  %1026 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1016, i64 noundef %1025)
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %61, ptr noundef nonnull align 8 dereferenceable(8) %1004, ptr noundef nonnull align 8 dereferenceable(8) %1015, ptr noundef nonnull align 8 dereferenceable(8) %1026)
  call void @llvm.lifetime.start.p0(i64 48, ptr %62) #10
  %1027 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %61, i64 noundef 0)
  %1028 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %61, i64 noundef 1)
  %1029 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %61, i64 noundef 2)
  call void @llvm.lifetime.start.p0(i64 8, ptr %63) #10
  store double 0.000000e+00, ptr %63, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %64) #10
  store double 0.000000e+00, ptr %64, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %65) #10
  store double 0.000000e+00, ptr %65, align 8, !tbaa !15
  call void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %62, ptr noundef nonnull align 8 dereferenceable(8) %1027, ptr noundef nonnull align 8 dereferenceable(8) %1028, ptr noundef nonnull align 8 dereferenceable(8) %1029, ptr noundef nonnull align 8 dereferenceable(8) %63, ptr noundef nonnull align 8 dereferenceable(8) %64, ptr noundef nonnull align 8 dereferenceable(8) %65)
  %1030 = load ptr, ptr %5, align 8, !tbaa !5
  %1031 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1030, i32 0, i32 12
  %1032 = load i32, ptr %6, align 4, !tbaa !9
  %1033 = zext i32 %1032 to i64
  %1034 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1031, i64 noundef %1033) #10
  %1035 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSEOS_(ptr noundef nonnull align 16 dereferenceable(48) %1034, ptr noundef nonnull align 16 dereferenceable(48) %62) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %65) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %64) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %63) #10
  call void @llvm.lifetime.end.p0(i64 48, ptr %62) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr %66) #10
  %1036 = load ptr, ptr %5, align 8, !tbaa !5
  %1037 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1036, i32 0, i32 15
  %1038 = load i32, ptr %6, align 4, !tbaa !9
  %1039 = zext i32 %1038 to i64
  %1040 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1037, i64 noundef %1039) #10
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %66, ptr noundef nonnull align 8 dereferenceable(96) %54, ptr noundef nonnull align 8 dereferenceable(96) %1040)
  %1041 = load ptr, ptr %5, align 8, !tbaa !5
  %1042 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1041, i32 0, i32 34
  %1043 = load i32, ptr %6, align 4, !tbaa !9
  %1044 = zext i32 %1043 to i64
  %1045 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1042, i64 noundef %1044) #10
  %1046 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %1045, ptr noundef nonnull align 8 dereferenceable(96) %66) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr %66) #10
  call void @llvm.lifetime.end.p0(i64 24, ptr %61) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr %54) #10
  br label %2318

1047:                                             ; preds = %964
  %1048 = load ptr, ptr %5, align 8, !tbaa !5
  %1049 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1048, i32 0, i32 10
  %1050 = load i32, ptr %6, align 4, !tbaa !9
  %1051 = zext i32 %1050 to i64
  %1052 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1049, i64 noundef %1051) #10
  %1053 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1052, i32 0, i32 1
  %1054 = load i32, ptr %1053, align 8, !tbaa !11
  %1055 = icmp eq i32 %1054, 7
  br i1 %1055, label %1056, label %1315

1056:                                             ; preds = %1047
  call void @llvm.lifetime.start.p0(i64 8, ptr %67) #10
  %1057 = load ptr, ptr %7, align 8, !tbaa !5
  %1058 = load ptr, ptr %5, align 8, !tbaa !5
  %1059 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1058, i32 0, i32 10
  %1060 = load i32, ptr %6, align 4, !tbaa !9
  %1061 = zext i32 %1060 to i64
  %1062 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1059, i64 noundef %1061) #10
  %1063 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1062, i32 0, i32 3
  %1064 = load i32, ptr %1063, align 8, !tbaa !14
  %1065 = zext i32 %1064 to i64
  %1066 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1057, i64 noundef %1065)
  %1067 = load double, ptr %1066, align 8, !tbaa !15
  store double %1067, ptr %67, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %68) #10
  %1068 = load ptr, ptr %7, align 8, !tbaa !5
  %1069 = load ptr, ptr %5, align 8, !tbaa !5
  %1070 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1069, i32 0, i32 10
  %1071 = load i32, ptr %6, align 4, !tbaa !9
  %1072 = zext i32 %1071 to i64
  %1073 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1070, i64 noundef %1072) #10
  %1074 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1073, i32 0, i32 3
  %1075 = load i32, ptr %1074, align 8, !tbaa !14
  %1076 = add i32 %1075, 1
  %1077 = zext i32 %1076 to i64
  %1078 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1068, i64 noundef %1077)
  %1079 = load double, ptr %1078, align 8, !tbaa !15
  store double %1079, ptr %68, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %69) #10
  %1080 = load ptr, ptr %7, align 8, !tbaa !5
  %1081 = load ptr, ptr %5, align 8, !tbaa !5
  %1082 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1081, i32 0, i32 10
  %1083 = load i32, ptr %6, align 4, !tbaa !9
  %1084 = zext i32 %1083 to i64
  %1085 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1082, i64 noundef %1084) #10
  %1086 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1085, i32 0, i32 3
  %1087 = load i32, ptr %1086, align 8, !tbaa !14
  %1088 = add i32 %1087, 2
  %1089 = zext i32 %1088 to i64
  %1090 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1080, i64 noundef %1089)
  %1091 = load double, ptr %1090, align 8, !tbaa !15
  store double %1091, ptr %69, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %70) #10
  %1092 = load double, ptr %67, align 8, !tbaa !15
  %1093 = call double @sin(double noundef %1092) #10
  store double %1093, ptr %70, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %71) #10
  %1094 = load double, ptr %67, align 8, !tbaa !15
  %1095 = call double @cos(double noundef %1094) #10
  store double %1095, ptr %71, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %72) #10
  %1096 = load double, ptr %68, align 8, !tbaa !15
  %1097 = call double @sin(double noundef %1096) #10
  store double %1097, ptr %72, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %73) #10
  %1098 = load double, ptr %68, align 8, !tbaa !15
  %1099 = call double @cos(double noundef %1098) #10
  store double %1099, ptr %73, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %74) #10
  %1100 = load double, ptr %69, align 8, !tbaa !15
  %1101 = call double @sin(double noundef %1100) #10
  store double %1101, ptr %74, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %75) #10
  %1102 = load double, ptr %69, align 8, !tbaa !15
  %1103 = call double @cos(double noundef %1102) #10
  store double %1103, ptr %75, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 96, ptr %76) #10
  call void @llvm.lifetime.start.p0(i64 72, ptr %77) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %78) #10
  %1104 = load double, ptr %71, align 8, !tbaa !15
  %1105 = load double, ptr %73, align 8, !tbaa !15
  %1106 = fmul double %1104, %1105
  store double %1106, ptr %78, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %79) #10
  %1107 = load double, ptr %70, align 8, !tbaa !15
  %1108 = load double, ptr %73, align 8, !tbaa !15
  %1109 = fmul double %1107, %1108
  store double %1109, ptr %79, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %80) #10
  %1110 = load double, ptr %72, align 8, !tbaa !15
  %1111 = fneg double %1110
  store double %1111, ptr %80, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %81) #10
  %1112 = load double, ptr %71, align 8, !tbaa !15
  %1113 = load double, ptr %72, align 8, !tbaa !15
  %1114 = fmul double %1112, %1113
  %1115 = load double, ptr %74, align 8, !tbaa !15
  %1116 = load double, ptr %70, align 8, !tbaa !15
  %1117 = load double, ptr %75, align 8, !tbaa !15
  %1118 = fmul double %1116, %1117
  %1119 = fneg double %1118
  %1120 = call double @llvm.fmuladd.f64(double %1114, double %1115, double %1119)
  store double %1120, ptr %81, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %82) #10
  %1121 = load double, ptr %70, align 8, !tbaa !15
  %1122 = load double, ptr %72, align 8, !tbaa !15
  %1123 = fmul double %1121, %1122
  %1124 = load double, ptr %74, align 8, !tbaa !15
  %1125 = load double, ptr %71, align 8, !tbaa !15
  %1126 = load double, ptr %75, align 8, !tbaa !15
  %1127 = fmul double %1125, %1126
  %1128 = call double @llvm.fmuladd.f64(double %1123, double %1124, double %1127)
  store double %1128, ptr %82, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %83) #10
  %1129 = load double, ptr %73, align 8, !tbaa !15
  %1130 = load double, ptr %74, align 8, !tbaa !15
  %1131 = fmul double %1129, %1130
  store double %1131, ptr %83, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %84) #10
  %1132 = load double, ptr %71, align 8, !tbaa !15
  %1133 = load double, ptr %72, align 8, !tbaa !15
  %1134 = fmul double %1132, %1133
  %1135 = load double, ptr %75, align 8, !tbaa !15
  %1136 = load double, ptr %70, align 8, !tbaa !15
  %1137 = load double, ptr %74, align 8, !tbaa !15
  %1138 = fmul double %1136, %1137
  %1139 = call double @llvm.fmuladd.f64(double %1134, double %1135, double %1138)
  store double %1139, ptr %84, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %85) #10
  %1140 = load double, ptr %70, align 8, !tbaa !15
  %1141 = load double, ptr %72, align 8, !tbaa !15
  %1142 = fmul double %1140, %1141
  %1143 = load double, ptr %75, align 8, !tbaa !15
  %1144 = load double, ptr %71, align 8, !tbaa !15
  %1145 = load double, ptr %74, align 8, !tbaa !15
  %1146 = fmul double %1144, %1145
  %1147 = fneg double %1146
  %1148 = call double @llvm.fmuladd.f64(double %1142, double %1143, double %1147)
  store double %1148, ptr %85, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %86) #10
  %1149 = load double, ptr %73, align 8, !tbaa !15
  %1150 = load double, ptr %75, align 8, !tbaa !15
  %1151 = fmul double %1149, %1150
  store double %1151, ptr %86, align 8, !tbaa !15
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %77, ptr noundef nonnull align 8 dereferenceable(8) %78, ptr noundef nonnull align 8 dereferenceable(8) %79, ptr noundef nonnull align 8 dereferenceable(8) %80, ptr noundef nonnull align 8 dereferenceable(8) %81, ptr noundef nonnull align 8 dereferenceable(8) %82, ptr noundef nonnull align 8 dereferenceable(8) %83, ptr noundef nonnull align 8 dereferenceable(8) %84, ptr noundef nonnull align 8 dereferenceable(8) %85, ptr noundef nonnull align 8 dereferenceable(8) %86)
  call void @llvm.lifetime.start.p0(i64 24, ptr %87) #10
  call void @llvm.lifetime.start.p0(i64 16, ptr %88) #10
  call void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE4ZeroEv(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseNullaryOp.149") align 8 %88)
  call void @_ZN9Vector3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %87, ptr noundef nonnull align 1 dereferenceable(1) %88)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %76, ptr noundef nonnull align 8 dereferenceable(72) %77, ptr noundef nonnull align 8 dereferenceable(24) %87)
  call void @llvm.lifetime.end.p0(i64 16, ptr %88) #10
  call void @llvm.lifetime.end.p0(i64 24, ptr %87) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %86) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %85) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %84) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %83) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %82) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %81) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %80) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %79) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %78) #10
  call void @llvm.lifetime.end.p0(i64 72, ptr %77) #10
  %1152 = load double, ptr %72, align 8, !tbaa !15
  %1153 = fneg double %1152
  %1154 = load ptr, ptr %5, align 8, !tbaa !5
  %1155 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1154, i32 0, i32 17
  %1156 = load i32, ptr %6, align 4, !tbaa !9
  %1157 = zext i32 %1156 to i64
  %1158 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1155, i64 noundef %1157) #10
  %1159 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1158, i64 noundef 0, i64 noundef 0)
  store double %1153, ptr %1159, align 8, !tbaa !15
  %1160 = load ptr, ptr %5, align 8, !tbaa !5
  %1161 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1160, i32 0, i32 17
  %1162 = load i32, ptr %6, align 4, !tbaa !9
  %1163 = zext i32 %1162 to i64
  %1164 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1161, i64 noundef %1163) #10
  %1165 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1164, i64 noundef 0, i64 noundef 2)
  store double 1.000000e+00, ptr %1165, align 8, !tbaa !15
  %1166 = load double, ptr %73, align 8, !tbaa !15
  %1167 = load double, ptr %74, align 8, !tbaa !15
  %1168 = fmul double %1166, %1167
  %1169 = load ptr, ptr %5, align 8, !tbaa !5
  %1170 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1169, i32 0, i32 17
  %1171 = load i32, ptr %6, align 4, !tbaa !9
  %1172 = zext i32 %1171 to i64
  %1173 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1170, i64 noundef %1172) #10
  %1174 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1173, i64 noundef 1, i64 noundef 0)
  store double %1168, ptr %1174, align 8, !tbaa !15
  %1175 = load double, ptr %75, align 8, !tbaa !15
  %1176 = load ptr, ptr %5, align 8, !tbaa !5
  %1177 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1176, i32 0, i32 17
  %1178 = load i32, ptr %6, align 4, !tbaa !9
  %1179 = zext i32 %1178 to i64
  %1180 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1177, i64 noundef %1179) #10
  %1181 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1180, i64 noundef 1, i64 noundef 1)
  store double %1175, ptr %1181, align 8, !tbaa !15
  %1182 = load double, ptr %73, align 8, !tbaa !15
  %1183 = load double, ptr %75, align 8, !tbaa !15
  %1184 = fmul double %1182, %1183
  %1185 = load ptr, ptr %5, align 8, !tbaa !5
  %1186 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1185, i32 0, i32 17
  %1187 = load i32, ptr %6, align 4, !tbaa !9
  %1188 = zext i32 %1187 to i64
  %1189 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1186, i64 noundef %1188) #10
  %1190 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1189, i64 noundef 2, i64 noundef 0)
  store double %1184, ptr %1190, align 8, !tbaa !15
  %1191 = load double, ptr %74, align 8, !tbaa !15
  %1192 = fneg double %1191
  %1193 = load ptr, ptr %5, align 8, !tbaa !5
  %1194 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1193, i32 0, i32 17
  %1195 = load i32, ptr %6, align 4, !tbaa !9
  %1196 = zext i32 %1195 to i64
  %1197 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1194, i64 noundef %1196) #10
  %1198 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1197, i64 noundef 2, i64 noundef 1)
  store double %1192, ptr %1198, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %89) #10
  %1199 = load ptr, ptr %8, align 8, !tbaa !5
  %1200 = load ptr, ptr %5, align 8, !tbaa !5
  %1201 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1200, i32 0, i32 10
  %1202 = load i32, ptr %6, align 4, !tbaa !9
  %1203 = zext i32 %1202 to i64
  %1204 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1201, i64 noundef %1203) #10
  %1205 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1204, i32 0, i32 3
  %1206 = load i32, ptr %1205, align 8, !tbaa !14
  %1207 = zext i32 %1206 to i64
  %1208 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1199, i64 noundef %1207)
  %1209 = load double, ptr %1208, align 8, !tbaa !15
  store double %1209, ptr %89, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %90) #10
  %1210 = load ptr, ptr %8, align 8, !tbaa !5
  %1211 = load ptr, ptr %5, align 8, !tbaa !5
  %1212 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1211, i32 0, i32 10
  %1213 = load i32, ptr %6, align 4, !tbaa !9
  %1214 = zext i32 %1213 to i64
  %1215 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1212, i64 noundef %1214) #10
  %1216 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1215, i32 0, i32 3
  %1217 = load i32, ptr %1216, align 8, !tbaa !14
  %1218 = add i32 %1217, 1
  %1219 = zext i32 %1218 to i64
  %1220 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1210, i64 noundef %1219)
  %1221 = load double, ptr %1220, align 8, !tbaa !15
  store double %1221, ptr %90, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %91) #10
  %1222 = load ptr, ptr %8, align 8, !tbaa !5
  %1223 = load ptr, ptr %5, align 8, !tbaa !5
  %1224 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1223, i32 0, i32 10
  %1225 = load i32, ptr %6, align 4, !tbaa !9
  %1226 = zext i32 %1225 to i64
  %1227 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1224, i64 noundef %1226) #10
  %1228 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1227, i32 0, i32 3
  %1229 = load i32, ptr %1228, align 8, !tbaa !14
  %1230 = add i32 %1229, 2
  %1231 = zext i32 %1230 to i64
  %1232 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1222, i64 noundef %1231)
  %1233 = load double, ptr %1232, align 8, !tbaa !15
  store double %1233, ptr %91, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 16, ptr %92) #10
  %1234 = load ptr, ptr %5, align 8, !tbaa !5
  %1235 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1234, i32 0, i32 17
  %1236 = load i32, ptr %6, align 4, !tbaa !9
  %1237 = zext i32 %1236 to i64
  %1238 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1235, i64 noundef %1237) #10
  call void @llvm.lifetime.start.p0(i64 24, ptr %93) #10
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %93, ptr noundef nonnull align 8 dereferenceable(8) %89, ptr noundef nonnull align 8 dereferenceable(8) %90, ptr noundef nonnull align 8 dereferenceable(8) %91)
  %1239 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEEEmlINS1_IdLi3ELi1ELi0ELi3ELi1EEEEEKNS_7ProductIS2_T_Li0EEERKNS0_IS7_EE(ptr noundef nonnull align 1 dereferenceable(1) %1238, ptr noundef nonnull align 1 dereferenceable(1) %93)
  %1240 = getelementptr inbounds { ptr, ptr }, ptr %92, i32 0, i32 0
  %1241 = extractvalue { ptr, ptr } %1239, 0
  store ptr %1241, ptr %1240, align 8
  %1242 = getelementptr inbounds { ptr, ptr }, ptr %92, i32 0, i32 1
  %1243 = extractvalue { ptr, ptr } %1239, 1
  store ptr %1243, ptr %1242, align 8
  %1244 = load ptr, ptr %5, align 8, !tbaa !5
  %1245 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1244, i32 0, i32 12
  %1246 = load i32, ptr %6, align 4, !tbaa !9
  %1247 = zext i32 %1246 to i64
  %1248 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1245, i64 noundef %1247) #10
  %1249 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen7ProductINS1_6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS3_IdLi3ELi1ELi0ELi3ELi1EEELi0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48) %1248, ptr noundef nonnull align 1 dereferenceable(1) %92)
  call void @llvm.lifetime.end.p0(i64 24, ptr %93) #10
  call void @llvm.lifetime.end.p0(i64 16, ptr %92) #10
  %1250 = load ptr, ptr %5, align 8, !tbaa !5
  %1251 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1250, i32 0, i32 13
  %1252 = load i32, ptr %6, align 4, !tbaa !9
  %1253 = zext i32 %1252 to i64
  %1254 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1251, i64 noundef %1253) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %94) #10
  %1255 = load double, ptr %73, align 8, !tbaa !15
  %1256 = fneg double %1255
  %1257 = load double, ptr %89, align 8, !tbaa !15
  %1258 = fmul double %1256, %1257
  %1259 = load double, ptr %90, align 8, !tbaa !15
  %1260 = fmul double %1258, %1259
  store double %1260, ptr %94, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %95) #10
  %1261 = load double, ptr %72, align 8, !tbaa !15
  %1262 = fneg double %1261
  %1263 = load double, ptr %74, align 8, !tbaa !15
  %1264 = fmul double %1262, %1263
  %1265 = load double, ptr %89, align 8, !tbaa !15
  %1266 = fmul double %1264, %1265
  %1267 = load double, ptr %90, align 8, !tbaa !15
  %1268 = load double, ptr %73, align 8, !tbaa !15
  %1269 = load double, ptr %75, align 8, !tbaa !15
  %1270 = fmul double %1268, %1269
  %1271 = load double, ptr %89, align 8, !tbaa !15
  %1272 = fmul double %1270, %1271
  %1273 = load double, ptr %91, align 8, !tbaa !15
  %1274 = fmul double %1272, %1273
  %1275 = call double @llvm.fmuladd.f64(double %1266, double %1267, double %1274)
  %1276 = load double, ptr %74, align 8, !tbaa !15
  %1277 = load double, ptr %90, align 8, !tbaa !15
  %1278 = fmul double %1276, %1277
  %1279 = load double, ptr %91, align 8, !tbaa !15
  %1280 = fneg double %1278
  %1281 = call double @llvm.fmuladd.f64(double %1280, double %1279, double %1275)
  store double %1281, ptr %95, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %96) #10
  %1282 = load double, ptr %72, align 8, !tbaa !15
  %1283 = fneg double %1282
  %1284 = load double, ptr %75, align 8, !tbaa !15
  %1285 = fmul double %1283, %1284
  %1286 = load double, ptr %89, align 8, !tbaa !15
  %1287 = fmul double %1285, %1286
  %1288 = load double, ptr %90, align 8, !tbaa !15
  %1289 = load double, ptr %73, align 8, !tbaa !15
  %1290 = load double, ptr %74, align 8, !tbaa !15
  %1291 = fmul double %1289, %1290
  %1292 = load double, ptr %89, align 8, !tbaa !15
  %1293 = fmul double %1291, %1292
  %1294 = load double, ptr %91, align 8, !tbaa !15
  %1295 = fmul double %1293, %1294
  %1296 = fneg double %1295
  %1297 = call double @llvm.fmuladd.f64(double %1287, double %1288, double %1296)
  %1298 = load double, ptr %75, align 8, !tbaa !15
  %1299 = load double, ptr %90, align 8, !tbaa !15
  %1300 = fmul double %1298, %1299
  %1301 = load double, ptr %91, align 8, !tbaa !15
  %1302 = fneg double %1300
  %1303 = call double @llvm.fmuladd.f64(double %1302, double %1301, double %1297)
  store double %1303, ptr %96, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %97) #10
  store double 0.000000e+00, ptr %97, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %98) #10
  store double 0.000000e+00, ptr %98, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %99) #10
  store double 0.000000e+00, ptr %99, align 8, !tbaa !15
  call void @_ZN15SpatialVector_t3setERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %1254, ptr noundef nonnull align 8 dereferenceable(8) %94, ptr noundef nonnull align 8 dereferenceable(8) %95, ptr noundef nonnull align 8 dereferenceable(8) %96, ptr noundef nonnull align 8 dereferenceable(8) %97, ptr noundef nonnull align 8 dereferenceable(8) %98, ptr noundef nonnull align 8 dereferenceable(8) %99)
  call void @llvm.lifetime.end.p0(i64 8, ptr %99) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %98) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %97) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %96) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %95) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %94) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr %100) #10
  %1304 = load ptr, ptr %5, align 8, !tbaa !5
  %1305 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1304, i32 0, i32 15
  %1306 = load i32, ptr %6, align 4, !tbaa !9
  %1307 = zext i32 %1306 to i64
  %1308 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1305, i64 noundef %1307) #10
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %100, ptr noundef nonnull align 8 dereferenceable(96) %76, ptr noundef nonnull align 8 dereferenceable(96) %1308)
  %1309 = load ptr, ptr %5, align 8, !tbaa !5
  %1310 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1309, i32 0, i32 34
  %1311 = load i32, ptr %6, align 4, !tbaa !9
  %1312 = zext i32 %1311 to i64
  %1313 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1310, i64 noundef %1312) #10
  %1314 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %1313, ptr noundef nonnull align 8 dereferenceable(96) %100) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr %100) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %91) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %90) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %89) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr %76) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %75) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %74) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %73) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %72) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %71) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %70) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %69) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %68) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %67) #10
  br label %2317

1315:                                             ; preds = %1047
  %1316 = load ptr, ptr %5, align 8, !tbaa !5
  %1317 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1316, i32 0, i32 10
  %1318 = load i32, ptr %6, align 4, !tbaa !9
  %1319 = zext i32 %1318 to i64
  %1320 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1317, i64 noundef %1319) #10
  %1321 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1320, i32 0, i32 1
  %1322 = load i32, ptr %1321, align 8, !tbaa !11
  %1323 = icmp eq i32 %1322, 8
  br i1 %1323, label %1324, label %1580

1324:                                             ; preds = %1315
  call void @llvm.lifetime.start.p0(i64 8, ptr %101) #10
  %1325 = load ptr, ptr %7, align 8, !tbaa !5
  %1326 = load ptr, ptr %5, align 8, !tbaa !5
  %1327 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1326, i32 0, i32 10
  %1328 = load i32, ptr %6, align 4, !tbaa !9
  %1329 = zext i32 %1328 to i64
  %1330 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1327, i64 noundef %1329) #10
  %1331 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1330, i32 0, i32 3
  %1332 = load i32, ptr %1331, align 8, !tbaa !14
  %1333 = zext i32 %1332 to i64
  %1334 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1325, i64 noundef %1333)
  %1335 = load double, ptr %1334, align 8, !tbaa !15
  store double %1335, ptr %101, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %102) #10
  %1336 = load ptr, ptr %7, align 8, !tbaa !5
  %1337 = load ptr, ptr %5, align 8, !tbaa !5
  %1338 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1337, i32 0, i32 10
  %1339 = load i32, ptr %6, align 4, !tbaa !9
  %1340 = zext i32 %1339 to i64
  %1341 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1338, i64 noundef %1340) #10
  %1342 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1341, i32 0, i32 3
  %1343 = load i32, ptr %1342, align 8, !tbaa !14
  %1344 = add i32 %1343, 1
  %1345 = zext i32 %1344 to i64
  %1346 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1336, i64 noundef %1345)
  %1347 = load double, ptr %1346, align 8, !tbaa !15
  store double %1347, ptr %102, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %103) #10
  %1348 = load ptr, ptr %7, align 8, !tbaa !5
  %1349 = load ptr, ptr %5, align 8, !tbaa !5
  %1350 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1349, i32 0, i32 10
  %1351 = load i32, ptr %6, align 4, !tbaa !9
  %1352 = zext i32 %1351 to i64
  %1353 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1350, i64 noundef %1352) #10
  %1354 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1353, i32 0, i32 3
  %1355 = load i32, ptr %1354, align 8, !tbaa !14
  %1356 = add i32 %1355, 2
  %1357 = zext i32 %1356 to i64
  %1358 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1348, i64 noundef %1357)
  %1359 = load double, ptr %1358, align 8, !tbaa !15
  store double %1359, ptr %103, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %104) #10
  %1360 = load double, ptr %101, align 8, !tbaa !15
  %1361 = call double @sin(double noundef %1360) #10
  store double %1361, ptr %104, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %105) #10
  %1362 = load double, ptr %101, align 8, !tbaa !15
  %1363 = call double @cos(double noundef %1362) #10
  store double %1363, ptr %105, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %106) #10
  %1364 = load double, ptr %102, align 8, !tbaa !15
  %1365 = call double @sin(double noundef %1364) #10
  store double %1365, ptr %106, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %107) #10
  %1366 = load double, ptr %102, align 8, !tbaa !15
  %1367 = call double @cos(double noundef %1366) #10
  store double %1367, ptr %107, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %108) #10
  %1368 = load double, ptr %103, align 8, !tbaa !15
  %1369 = call double @sin(double noundef %1368) #10
  store double %1369, ptr %108, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %109) #10
  %1370 = load double, ptr %103, align 8, !tbaa !15
  %1371 = call double @cos(double noundef %1370) #10
  store double %1371, ptr %109, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 96, ptr %110) #10
  call void @llvm.lifetime.start.p0(i64 72, ptr %111) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %112) #10
  %1372 = load double, ptr %109, align 8, !tbaa !15
  %1373 = load double, ptr %107, align 8, !tbaa !15
  %1374 = fmul double %1372, %1373
  store double %1374, ptr %112, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %113) #10
  %1375 = load double, ptr %108, align 8, !tbaa !15
  %1376 = load double, ptr %105, align 8, !tbaa !15
  %1377 = load double, ptr %109, align 8, !tbaa !15
  %1378 = load double, ptr %106, align 8, !tbaa !15
  %1379 = fmul double %1377, %1378
  %1380 = load double, ptr %104, align 8, !tbaa !15
  %1381 = fmul double %1379, %1380
  %1382 = call double @llvm.fmuladd.f64(double %1375, double %1376, double %1381)
  store double %1382, ptr %113, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %114) #10
  %1383 = load double, ptr %108, align 8, !tbaa !15
  %1384 = load double, ptr %104, align 8, !tbaa !15
  %1385 = load double, ptr %109, align 8, !tbaa !15
  %1386 = load double, ptr %106, align 8, !tbaa !15
  %1387 = fmul double %1385, %1386
  %1388 = load double, ptr %105, align 8, !tbaa !15
  %1389 = fmul double %1387, %1388
  %1390 = fneg double %1389
  %1391 = call double @llvm.fmuladd.f64(double %1383, double %1384, double %1390)
  store double %1391, ptr %114, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %115) #10
  %1392 = load double, ptr %108, align 8, !tbaa !15
  %1393 = fneg double %1392
  %1394 = load double, ptr %107, align 8, !tbaa !15
  %1395 = fmul double %1393, %1394
  store double %1395, ptr %115, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %116) #10
  %1396 = load double, ptr %109, align 8, !tbaa !15
  %1397 = load double, ptr %105, align 8, !tbaa !15
  %1398 = load double, ptr %108, align 8, !tbaa !15
  %1399 = load double, ptr %106, align 8, !tbaa !15
  %1400 = fmul double %1398, %1399
  %1401 = load double, ptr %104, align 8, !tbaa !15
  %1402 = fmul double %1400, %1401
  %1403 = fneg double %1402
  %1404 = call double @llvm.fmuladd.f64(double %1396, double %1397, double %1403)
  store double %1404, ptr %116, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %117) #10
  %1405 = load double, ptr %109, align 8, !tbaa !15
  %1406 = load double, ptr %104, align 8, !tbaa !15
  %1407 = load double, ptr %108, align 8, !tbaa !15
  %1408 = load double, ptr %106, align 8, !tbaa !15
  %1409 = fmul double %1407, %1408
  %1410 = load double, ptr %105, align 8, !tbaa !15
  %1411 = fmul double %1409, %1410
  %1412 = call double @llvm.fmuladd.f64(double %1405, double %1406, double %1411)
  store double %1412, ptr %117, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %118) #10
  %1413 = load double, ptr %107, align 8, !tbaa !15
  %1414 = fneg double %1413
  %1415 = load double, ptr %104, align 8, !tbaa !15
  %1416 = fmul double %1414, %1415
  store double %1416, ptr %118, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %119) #10
  %1417 = load double, ptr %107, align 8, !tbaa !15
  %1418 = load double, ptr %105, align 8, !tbaa !15
  %1419 = fmul double %1417, %1418
  store double %1419, ptr %119, align 8, !tbaa !15
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %111, ptr noundef nonnull align 8 dereferenceable(8) %112, ptr noundef nonnull align 8 dereferenceable(8) %113, ptr noundef nonnull align 8 dereferenceable(8) %114, ptr noundef nonnull align 8 dereferenceable(8) %115, ptr noundef nonnull align 8 dereferenceable(8) %116, ptr noundef nonnull align 8 dereferenceable(8) %117, ptr noundef nonnull align 8 dereferenceable(8) %106, ptr noundef nonnull align 8 dereferenceable(8) %118, ptr noundef nonnull align 8 dereferenceable(8) %119)
  call void @llvm.lifetime.start.p0(i64 24, ptr %120) #10
  call void @llvm.lifetime.start.p0(i64 16, ptr %121) #10
  call void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE4ZeroEv(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseNullaryOp.149") align 8 %121)
  call void @_ZN9Vector3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %120, ptr noundef nonnull align 1 dereferenceable(1) %121)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %110, ptr noundef nonnull align 8 dereferenceable(72) %111, ptr noundef nonnull align 8 dereferenceable(24) %120)
  call void @llvm.lifetime.end.p0(i64 16, ptr %121) #10
  call void @llvm.lifetime.end.p0(i64 24, ptr %120) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %119) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %118) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %117) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %116) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %115) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %114) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %113) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %112) #10
  call void @llvm.lifetime.end.p0(i64 72, ptr %111) #10
  %1420 = load double, ptr %109, align 8, !tbaa !15
  %1421 = load double, ptr %107, align 8, !tbaa !15
  %1422 = fmul double %1420, %1421
  %1423 = load ptr, ptr %5, align 8, !tbaa !5
  %1424 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1423, i32 0, i32 17
  %1425 = load i32, ptr %6, align 4, !tbaa !9
  %1426 = zext i32 %1425 to i64
  %1427 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1424, i64 noundef %1426) #10
  %1428 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1427, i64 noundef 0, i64 noundef 0)
  store double %1422, ptr %1428, align 8, !tbaa !15
  %1429 = load double, ptr %108, align 8, !tbaa !15
  %1430 = load ptr, ptr %5, align 8, !tbaa !5
  %1431 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1430, i32 0, i32 17
  %1432 = load i32, ptr %6, align 4, !tbaa !9
  %1433 = zext i32 %1432 to i64
  %1434 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1431, i64 noundef %1433) #10
  %1435 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1434, i64 noundef 0, i64 noundef 1)
  store double %1429, ptr %1435, align 8, !tbaa !15
  %1436 = load double, ptr %108, align 8, !tbaa !15
  %1437 = fneg double %1436
  %1438 = load double, ptr %107, align 8, !tbaa !15
  %1439 = fmul double %1437, %1438
  %1440 = load ptr, ptr %5, align 8, !tbaa !5
  %1441 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1440, i32 0, i32 17
  %1442 = load i32, ptr %6, align 4, !tbaa !9
  %1443 = zext i32 %1442 to i64
  %1444 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1441, i64 noundef %1443) #10
  %1445 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1444, i64 noundef 1, i64 noundef 0)
  store double %1439, ptr %1445, align 8, !tbaa !15
  %1446 = load double, ptr %109, align 8, !tbaa !15
  %1447 = load ptr, ptr %5, align 8, !tbaa !5
  %1448 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1447, i32 0, i32 17
  %1449 = load i32, ptr %6, align 4, !tbaa !9
  %1450 = zext i32 %1449 to i64
  %1451 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1448, i64 noundef %1450) #10
  %1452 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1451, i64 noundef 1, i64 noundef 1)
  store double %1446, ptr %1452, align 8, !tbaa !15
  %1453 = load double, ptr %106, align 8, !tbaa !15
  %1454 = load ptr, ptr %5, align 8, !tbaa !5
  %1455 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1454, i32 0, i32 17
  %1456 = load i32, ptr %6, align 4, !tbaa !9
  %1457 = zext i32 %1456 to i64
  %1458 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1455, i64 noundef %1457) #10
  %1459 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1458, i64 noundef 2, i64 noundef 0)
  store double %1453, ptr %1459, align 8, !tbaa !15
  %1460 = load ptr, ptr %5, align 8, !tbaa !5
  %1461 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1460, i32 0, i32 17
  %1462 = load i32, ptr %6, align 4, !tbaa !9
  %1463 = zext i32 %1462 to i64
  %1464 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1461, i64 noundef %1463) #10
  %1465 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1464, i64 noundef 2, i64 noundef 2)
  store double 1.000000e+00, ptr %1465, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %122) #10
  %1466 = load ptr, ptr %8, align 8, !tbaa !5
  %1467 = load ptr, ptr %5, align 8, !tbaa !5
  %1468 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1467, i32 0, i32 10
  %1469 = load i32, ptr %6, align 4, !tbaa !9
  %1470 = zext i32 %1469 to i64
  %1471 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1468, i64 noundef %1470) #10
  %1472 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1471, i32 0, i32 3
  %1473 = load i32, ptr %1472, align 8, !tbaa !14
  %1474 = zext i32 %1473 to i64
  %1475 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1466, i64 noundef %1474)
  %1476 = load double, ptr %1475, align 8, !tbaa !15
  store double %1476, ptr %122, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %123) #10
  %1477 = load ptr, ptr %8, align 8, !tbaa !5
  %1478 = load ptr, ptr %5, align 8, !tbaa !5
  %1479 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1478, i32 0, i32 10
  %1480 = load i32, ptr %6, align 4, !tbaa !9
  %1481 = zext i32 %1480 to i64
  %1482 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1479, i64 noundef %1481) #10
  %1483 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1482, i32 0, i32 3
  %1484 = load i32, ptr %1483, align 8, !tbaa !14
  %1485 = add i32 %1484, 1
  %1486 = zext i32 %1485 to i64
  %1487 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1477, i64 noundef %1486)
  %1488 = load double, ptr %1487, align 8, !tbaa !15
  store double %1488, ptr %123, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %124) #10
  %1489 = load ptr, ptr %8, align 8, !tbaa !5
  %1490 = load ptr, ptr %5, align 8, !tbaa !5
  %1491 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1490, i32 0, i32 10
  %1492 = load i32, ptr %6, align 4, !tbaa !9
  %1493 = zext i32 %1492 to i64
  %1494 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1491, i64 noundef %1493) #10
  %1495 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1494, i32 0, i32 3
  %1496 = load i32, ptr %1495, align 8, !tbaa !14
  %1497 = add i32 %1496, 2
  %1498 = zext i32 %1497 to i64
  %1499 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1489, i64 noundef %1498)
  %1500 = load double, ptr %1499, align 8, !tbaa !15
  store double %1500, ptr %124, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 16, ptr %125) #10
  %1501 = load ptr, ptr %5, align 8, !tbaa !5
  %1502 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1501, i32 0, i32 17
  %1503 = load i32, ptr %6, align 4, !tbaa !9
  %1504 = zext i32 %1503 to i64
  %1505 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1502, i64 noundef %1504) #10
  call void @llvm.lifetime.start.p0(i64 24, ptr %126) #10
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %126, ptr noundef nonnull align 8 dereferenceable(8) %122, ptr noundef nonnull align 8 dereferenceable(8) %123, ptr noundef nonnull align 8 dereferenceable(8) %124)
  %1506 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEEEmlINS1_IdLi3ELi1ELi0ELi3ELi1EEEEEKNS_7ProductIS2_T_Li0EEERKNS0_IS7_EE(ptr noundef nonnull align 1 dereferenceable(1) %1505, ptr noundef nonnull align 1 dereferenceable(1) %126)
  %1507 = getelementptr inbounds { ptr, ptr }, ptr %125, i32 0, i32 0
  %1508 = extractvalue { ptr, ptr } %1506, 0
  store ptr %1508, ptr %1507, align 8
  %1509 = getelementptr inbounds { ptr, ptr }, ptr %125, i32 0, i32 1
  %1510 = extractvalue { ptr, ptr } %1506, 1
  store ptr %1510, ptr %1509, align 8
  %1511 = load ptr, ptr %5, align 8, !tbaa !5
  %1512 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1511, i32 0, i32 12
  %1513 = load i32, ptr %6, align 4, !tbaa !9
  %1514 = zext i32 %1513 to i64
  %1515 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1512, i64 noundef %1514) #10
  %1516 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen7ProductINS1_6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS3_IdLi3ELi1ELi0ELi3ELi1EEELi0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48) %1515, ptr noundef nonnull align 1 dereferenceable(1) %125)
  call void @llvm.lifetime.end.p0(i64 24, ptr %126) #10
  call void @llvm.lifetime.end.p0(i64 16, ptr %125) #10
  %1517 = load ptr, ptr %5, align 8, !tbaa !5
  %1518 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1517, i32 0, i32 13
  %1519 = load i32, ptr %6, align 4, !tbaa !9
  %1520 = zext i32 %1519 to i64
  %1521 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1518, i64 noundef %1520) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %127) #10
  %1522 = load double, ptr %108, align 8, !tbaa !15
  %1523 = fneg double %1522
  %1524 = load double, ptr %107, align 8, !tbaa !15
  %1525 = fmul double %1523, %1524
  %1526 = load double, ptr %124, align 8, !tbaa !15
  %1527 = fmul double %1525, %1526
  %1528 = load double, ptr %122, align 8, !tbaa !15
  %1529 = load double, ptr %109, align 8, !tbaa !15
  %1530 = load double, ptr %106, align 8, !tbaa !15
  %1531 = fmul double %1529, %1530
  %1532 = load double, ptr %123, align 8, !tbaa !15
  %1533 = fmul double %1531, %1532
  %1534 = load double, ptr %122, align 8, !tbaa !15
  %1535 = fmul double %1533, %1534
  %1536 = fneg double %1535
  %1537 = call double @llvm.fmuladd.f64(double %1527, double %1528, double %1536)
  %1538 = load double, ptr %109, align 8, !tbaa !15
  %1539 = load double, ptr %124, align 8, !tbaa !15
  %1540 = fmul double %1538, %1539
  %1541 = load double, ptr %123, align 8, !tbaa !15
  %1542 = call double @llvm.fmuladd.f64(double %1540, double %1541, double %1537)
  store double %1542, ptr %127, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %128) #10
  %1543 = load double, ptr %109, align 8, !tbaa !15
  %1544 = fneg double %1543
  %1545 = load double, ptr %107, align 8, !tbaa !15
  %1546 = fmul double %1544, %1545
  %1547 = load double, ptr %124, align 8, !tbaa !15
  %1548 = fmul double %1546, %1547
  %1549 = load double, ptr %122, align 8, !tbaa !15
  %1550 = load double, ptr %108, align 8, !tbaa !15
  %1551 = load double, ptr %106, align 8, !tbaa !15
  %1552 = fmul double %1550, %1551
  %1553 = load double, ptr %123, align 8, !tbaa !15
  %1554 = fmul double %1552, %1553
  %1555 = load double, ptr %122, align 8, !tbaa !15
  %1556 = fmul double %1554, %1555
  %1557 = call double @llvm.fmuladd.f64(double %1548, double %1549, double %1556)
  %1558 = load double, ptr %108, align 8, !tbaa !15
  %1559 = load double, ptr %124, align 8, !tbaa !15
  %1560 = fmul double %1558, %1559
  %1561 = load double, ptr %123, align 8, !tbaa !15
  %1562 = fneg double %1560
  %1563 = call double @llvm.fmuladd.f64(double %1562, double %1561, double %1557)
  store double %1563, ptr %128, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %129) #10
  %1564 = load double, ptr %107, align 8, !tbaa !15
  %1565 = load double, ptr %123, align 8, !tbaa !15
  %1566 = fmul double %1564, %1565
  %1567 = load double, ptr %122, align 8, !tbaa !15
  %1568 = fmul double %1566, %1567
  store double %1568, ptr %129, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %130) #10
  store double 0.000000e+00, ptr %130, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %131) #10
  store double 0.000000e+00, ptr %131, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %132) #10
  store double 0.000000e+00, ptr %132, align 8, !tbaa !15
  call void @_ZN15SpatialVector_t3setERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %1521, ptr noundef nonnull align 8 dereferenceable(8) %127, ptr noundef nonnull align 8 dereferenceable(8) %128, ptr noundef nonnull align 8 dereferenceable(8) %129, ptr noundef nonnull align 8 dereferenceable(8) %130, ptr noundef nonnull align 8 dereferenceable(8) %131, ptr noundef nonnull align 8 dereferenceable(8) %132)
  call void @llvm.lifetime.end.p0(i64 8, ptr %132) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %131) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %130) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %129) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %128) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %127) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr %133) #10
  %1569 = load ptr, ptr %5, align 8, !tbaa !5
  %1570 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1569, i32 0, i32 15
  %1571 = load i32, ptr %6, align 4, !tbaa !9
  %1572 = zext i32 %1571 to i64
  %1573 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1570, i64 noundef %1572) #10
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %133, ptr noundef nonnull align 8 dereferenceable(96) %110, ptr noundef nonnull align 8 dereferenceable(96) %1573)
  %1574 = load ptr, ptr %5, align 8, !tbaa !5
  %1575 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1574, i32 0, i32 34
  %1576 = load i32, ptr %6, align 4, !tbaa !9
  %1577 = zext i32 %1576 to i64
  %1578 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1575, i64 noundef %1577) #10
  %1579 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %1578, ptr noundef nonnull align 8 dereferenceable(96) %133) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr %133) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %124) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %123) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %122) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr %110) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %109) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %108) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %107) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %106) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %105) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %104) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %103) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %102) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %101) #10
  br label %2316

1580:                                             ; preds = %1315
  %1581 = load ptr, ptr %5, align 8, !tbaa !5
  %1582 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1581, i32 0, i32 10
  %1583 = load i32, ptr %6, align 4, !tbaa !9
  %1584 = zext i32 %1583 to i64
  %1585 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1582, i64 noundef %1584) #10
  %1586 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1585, i32 0, i32 1
  %1587 = load i32, ptr %1586, align 8, !tbaa !11
  %1588 = icmp eq i32 %1587, 9
  br i1 %1588, label %1589, label %1848

1589:                                             ; preds = %1580
  call void @llvm.lifetime.start.p0(i64 8, ptr %134) #10
  %1590 = load ptr, ptr %7, align 8, !tbaa !5
  %1591 = load ptr, ptr %5, align 8, !tbaa !5
  %1592 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1591, i32 0, i32 10
  %1593 = load i32, ptr %6, align 4, !tbaa !9
  %1594 = zext i32 %1593 to i64
  %1595 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1592, i64 noundef %1594) #10
  %1596 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1595, i32 0, i32 3
  %1597 = load i32, ptr %1596, align 8, !tbaa !14
  %1598 = zext i32 %1597 to i64
  %1599 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1590, i64 noundef %1598)
  %1600 = load double, ptr %1599, align 8, !tbaa !15
  store double %1600, ptr %134, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %135) #10
  %1601 = load ptr, ptr %7, align 8, !tbaa !5
  %1602 = load ptr, ptr %5, align 8, !tbaa !5
  %1603 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1602, i32 0, i32 10
  %1604 = load i32, ptr %6, align 4, !tbaa !9
  %1605 = zext i32 %1604 to i64
  %1606 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1603, i64 noundef %1605) #10
  %1607 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1606, i32 0, i32 3
  %1608 = load i32, ptr %1607, align 8, !tbaa !14
  %1609 = add i32 %1608, 1
  %1610 = zext i32 %1609 to i64
  %1611 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1601, i64 noundef %1610)
  %1612 = load double, ptr %1611, align 8, !tbaa !15
  store double %1612, ptr %135, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %136) #10
  %1613 = load ptr, ptr %7, align 8, !tbaa !5
  %1614 = load ptr, ptr %5, align 8, !tbaa !5
  %1615 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1614, i32 0, i32 10
  %1616 = load i32, ptr %6, align 4, !tbaa !9
  %1617 = zext i32 %1616 to i64
  %1618 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1615, i64 noundef %1617) #10
  %1619 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1618, i32 0, i32 3
  %1620 = load i32, ptr %1619, align 8, !tbaa !14
  %1621 = add i32 %1620, 2
  %1622 = zext i32 %1621 to i64
  %1623 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1613, i64 noundef %1622)
  %1624 = load double, ptr %1623, align 8, !tbaa !15
  store double %1624, ptr %136, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %137) #10
  %1625 = load double, ptr %134, align 8, !tbaa !15
  %1626 = call double @sin(double noundef %1625) #10
  store double %1626, ptr %137, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %138) #10
  %1627 = load double, ptr %134, align 8, !tbaa !15
  %1628 = call double @cos(double noundef %1627) #10
  store double %1628, ptr %138, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %139) #10
  %1629 = load double, ptr %135, align 8, !tbaa !15
  %1630 = call double @sin(double noundef %1629) #10
  store double %1630, ptr %139, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %140) #10
  %1631 = load double, ptr %135, align 8, !tbaa !15
  %1632 = call double @cos(double noundef %1631) #10
  store double %1632, ptr %140, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %141) #10
  %1633 = load double, ptr %136, align 8, !tbaa !15
  %1634 = call double @sin(double noundef %1633) #10
  store double %1634, ptr %141, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %142) #10
  %1635 = load double, ptr %136, align 8, !tbaa !15
  %1636 = call double @cos(double noundef %1635) #10
  store double %1636, ptr %142, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 96, ptr %143) #10
  call void @llvm.lifetime.start.p0(i64 72, ptr %144) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %145) #10
  %1637 = load double, ptr %142, align 8, !tbaa !15
  %1638 = load double, ptr %138, align 8, !tbaa !15
  %1639 = load double, ptr %141, align 8, !tbaa !15
  %1640 = load double, ptr %139, align 8, !tbaa !15
  %1641 = fmul double %1639, %1640
  %1642 = load double, ptr %137, align 8, !tbaa !15
  %1643 = fmul double %1641, %1642
  %1644 = call double @llvm.fmuladd.f64(double %1637, double %1638, double %1643)
  store double %1644, ptr %145, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %146) #10
  %1645 = load double, ptr %141, align 8, !tbaa !15
  %1646 = load double, ptr %140, align 8, !tbaa !15
  %1647 = fmul double %1645, %1646
  store double %1647, ptr %146, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %147) #10
  %1648 = load double, ptr %142, align 8, !tbaa !15
  %1649 = fneg double %1648
  %1650 = load double, ptr %137, align 8, !tbaa !15
  %1651 = load double, ptr %141, align 8, !tbaa !15
  %1652 = load double, ptr %139, align 8, !tbaa !15
  %1653 = fmul double %1651, %1652
  %1654 = load double, ptr %138, align 8, !tbaa !15
  %1655 = fmul double %1653, %1654
  %1656 = call double @llvm.fmuladd.f64(double %1649, double %1650, double %1655)
  store double %1656, ptr %147, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %148) #10
  %1657 = load double, ptr %141, align 8, !tbaa !15
  %1658 = fneg double %1657
  %1659 = load double, ptr %138, align 8, !tbaa !15
  %1660 = load double, ptr %142, align 8, !tbaa !15
  %1661 = load double, ptr %139, align 8, !tbaa !15
  %1662 = fmul double %1660, %1661
  %1663 = load double, ptr %137, align 8, !tbaa !15
  %1664 = fmul double %1662, %1663
  %1665 = call double @llvm.fmuladd.f64(double %1658, double %1659, double %1664)
  store double %1665, ptr %148, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %149) #10
  %1666 = load double, ptr %142, align 8, !tbaa !15
  %1667 = load double, ptr %140, align 8, !tbaa !15
  %1668 = fmul double %1666, %1667
  store double %1668, ptr %149, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %150) #10
  %1669 = load double, ptr %141, align 8, !tbaa !15
  %1670 = load double, ptr %137, align 8, !tbaa !15
  %1671 = load double, ptr %142, align 8, !tbaa !15
  %1672 = load double, ptr %139, align 8, !tbaa !15
  %1673 = fmul double %1671, %1672
  %1674 = load double, ptr %138, align 8, !tbaa !15
  %1675 = fmul double %1673, %1674
  %1676 = call double @llvm.fmuladd.f64(double %1669, double %1670, double %1675)
  store double %1676, ptr %150, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %151) #10
  %1677 = load double, ptr %140, align 8, !tbaa !15
  %1678 = load double, ptr %137, align 8, !tbaa !15
  %1679 = fmul double %1677, %1678
  store double %1679, ptr %151, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %152) #10
  %1680 = load double, ptr %139, align 8, !tbaa !15
  %1681 = fneg double %1680
  store double %1681, ptr %152, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %153) #10
  %1682 = load double, ptr %140, align 8, !tbaa !15
  %1683 = load double, ptr %138, align 8, !tbaa !15
  %1684 = fmul double %1682, %1683
  store double %1684, ptr %153, align 8, !tbaa !15
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %144, ptr noundef nonnull align 8 dereferenceable(8) %145, ptr noundef nonnull align 8 dereferenceable(8) %146, ptr noundef nonnull align 8 dereferenceable(8) %147, ptr noundef nonnull align 8 dereferenceable(8) %148, ptr noundef nonnull align 8 dereferenceable(8) %149, ptr noundef nonnull align 8 dereferenceable(8) %150, ptr noundef nonnull align 8 dereferenceable(8) %151, ptr noundef nonnull align 8 dereferenceable(8) %152, ptr noundef nonnull align 8 dereferenceable(8) %153)
  call void @llvm.lifetime.start.p0(i64 24, ptr %154) #10
  call void @llvm.lifetime.start.p0(i64 16, ptr %155) #10
  call void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE4ZeroEv(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseNullaryOp.149") align 8 %155)
  call void @_ZN9Vector3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %154, ptr noundef nonnull align 1 dereferenceable(1) %155)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %143, ptr noundef nonnull align 8 dereferenceable(72) %144, ptr noundef nonnull align 8 dereferenceable(24) %154)
  call void @llvm.lifetime.end.p0(i64 16, ptr %155) #10
  call void @llvm.lifetime.end.p0(i64 24, ptr %154) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %153) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %152) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %151) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %150) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %149) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %148) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %147) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %146) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %145) #10
  call void @llvm.lifetime.end.p0(i64 72, ptr %144) #10
  %1685 = load double, ptr %141, align 8, !tbaa !15
  %1686 = load double, ptr %140, align 8, !tbaa !15
  %1687 = fmul double %1685, %1686
  %1688 = load ptr, ptr %5, align 8, !tbaa !5
  %1689 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1688, i32 0, i32 17
  %1690 = load i32, ptr %6, align 4, !tbaa !9
  %1691 = zext i32 %1690 to i64
  %1692 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1689, i64 noundef %1691) #10
  %1693 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1692, i64 noundef 0, i64 noundef 0)
  store double %1687, ptr %1693, align 8, !tbaa !15
  %1694 = load double, ptr %142, align 8, !tbaa !15
  %1695 = load ptr, ptr %5, align 8, !tbaa !5
  %1696 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1695, i32 0, i32 17
  %1697 = load i32, ptr %6, align 4, !tbaa !9
  %1698 = zext i32 %1697 to i64
  %1699 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1696, i64 noundef %1698) #10
  %1700 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1699, i64 noundef 0, i64 noundef 1)
  store double %1694, ptr %1700, align 8, !tbaa !15
  %1701 = load double, ptr %142, align 8, !tbaa !15
  %1702 = load double, ptr %140, align 8, !tbaa !15
  %1703 = fmul double %1701, %1702
  %1704 = load ptr, ptr %5, align 8, !tbaa !5
  %1705 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1704, i32 0, i32 17
  %1706 = load i32, ptr %6, align 4, !tbaa !9
  %1707 = zext i32 %1706 to i64
  %1708 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1705, i64 noundef %1707) #10
  %1709 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1708, i64 noundef 1, i64 noundef 0)
  store double %1703, ptr %1709, align 8, !tbaa !15
  %1710 = load double, ptr %141, align 8, !tbaa !15
  %1711 = fneg double %1710
  %1712 = load ptr, ptr %5, align 8, !tbaa !5
  %1713 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1712, i32 0, i32 17
  %1714 = load i32, ptr %6, align 4, !tbaa !9
  %1715 = zext i32 %1714 to i64
  %1716 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1713, i64 noundef %1715) #10
  %1717 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1716, i64 noundef 1, i64 noundef 1)
  store double %1711, ptr %1717, align 8, !tbaa !15
  %1718 = load double, ptr %139, align 8, !tbaa !15
  %1719 = fneg double %1718
  %1720 = load ptr, ptr %5, align 8, !tbaa !5
  %1721 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1720, i32 0, i32 17
  %1722 = load i32, ptr %6, align 4, !tbaa !9
  %1723 = zext i32 %1722 to i64
  %1724 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1721, i64 noundef %1723) #10
  %1725 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1724, i64 noundef 2, i64 noundef 0)
  store double %1719, ptr %1725, align 8, !tbaa !15
  %1726 = load ptr, ptr %5, align 8, !tbaa !5
  %1727 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1726, i32 0, i32 17
  %1728 = load i32, ptr %6, align 4, !tbaa !9
  %1729 = zext i32 %1728 to i64
  %1730 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1727, i64 noundef %1729) #10
  %1731 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1730, i64 noundef 2, i64 noundef 2)
  store double 1.000000e+00, ptr %1731, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %156) #10
  %1732 = load ptr, ptr %8, align 8, !tbaa !5
  %1733 = load ptr, ptr %5, align 8, !tbaa !5
  %1734 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1733, i32 0, i32 10
  %1735 = load i32, ptr %6, align 4, !tbaa !9
  %1736 = zext i32 %1735 to i64
  %1737 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1734, i64 noundef %1736) #10
  %1738 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1737, i32 0, i32 3
  %1739 = load i32, ptr %1738, align 8, !tbaa !14
  %1740 = zext i32 %1739 to i64
  %1741 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1732, i64 noundef %1740)
  %1742 = load double, ptr %1741, align 8, !tbaa !15
  store double %1742, ptr %156, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %157) #10
  %1743 = load ptr, ptr %8, align 8, !tbaa !5
  %1744 = load ptr, ptr %5, align 8, !tbaa !5
  %1745 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1744, i32 0, i32 10
  %1746 = load i32, ptr %6, align 4, !tbaa !9
  %1747 = zext i32 %1746 to i64
  %1748 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1745, i64 noundef %1747) #10
  %1749 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1748, i32 0, i32 3
  %1750 = load i32, ptr %1749, align 8, !tbaa !14
  %1751 = add i32 %1750, 1
  %1752 = zext i32 %1751 to i64
  %1753 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1743, i64 noundef %1752)
  %1754 = load double, ptr %1753, align 8, !tbaa !15
  store double %1754, ptr %157, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %158) #10
  %1755 = load ptr, ptr %8, align 8, !tbaa !5
  %1756 = load ptr, ptr %5, align 8, !tbaa !5
  %1757 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1756, i32 0, i32 10
  %1758 = load i32, ptr %6, align 4, !tbaa !9
  %1759 = zext i32 %1758 to i64
  %1760 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1757, i64 noundef %1759) #10
  %1761 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1760, i32 0, i32 3
  %1762 = load i32, ptr %1761, align 8, !tbaa !14
  %1763 = add i32 %1762, 2
  %1764 = zext i32 %1763 to i64
  %1765 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1755, i64 noundef %1764)
  %1766 = load double, ptr %1765, align 8, !tbaa !15
  store double %1766, ptr %158, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 16, ptr %159) #10
  %1767 = load ptr, ptr %5, align 8, !tbaa !5
  %1768 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1767, i32 0, i32 17
  %1769 = load i32, ptr %6, align 4, !tbaa !9
  %1770 = zext i32 %1769 to i64
  %1771 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1768, i64 noundef %1770) #10
  call void @llvm.lifetime.start.p0(i64 24, ptr %160) #10
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %160, ptr noundef nonnull align 8 dereferenceable(8) %156, ptr noundef nonnull align 8 dereferenceable(8) %157, ptr noundef nonnull align 8 dereferenceable(8) %158)
  %1772 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEEEmlINS1_IdLi3ELi1ELi0ELi3ELi1EEEEEKNS_7ProductIS2_T_Li0EEERKNS0_IS7_EE(ptr noundef nonnull align 1 dereferenceable(1) %1771, ptr noundef nonnull align 1 dereferenceable(1) %160)
  %1773 = getelementptr inbounds { ptr, ptr }, ptr %159, i32 0, i32 0
  %1774 = extractvalue { ptr, ptr } %1772, 0
  store ptr %1774, ptr %1773, align 8
  %1775 = getelementptr inbounds { ptr, ptr }, ptr %159, i32 0, i32 1
  %1776 = extractvalue { ptr, ptr } %1772, 1
  store ptr %1776, ptr %1775, align 8
  %1777 = load ptr, ptr %5, align 8, !tbaa !5
  %1778 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1777, i32 0, i32 12
  %1779 = load i32, ptr %6, align 4, !tbaa !9
  %1780 = zext i32 %1779 to i64
  %1781 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1778, i64 noundef %1780) #10
  %1782 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen7ProductINS1_6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS3_IdLi3ELi1ELi0ELi3ELi1EEELi0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48) %1781, ptr noundef nonnull align 1 dereferenceable(1) %159)
  call void @llvm.lifetime.end.p0(i64 24, ptr %160) #10
  call void @llvm.lifetime.end.p0(i64 16, ptr %159) #10
  %1783 = load ptr, ptr %5, align 8, !tbaa !5
  %1784 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1783, i32 0, i32 13
  %1785 = load i32, ptr %6, align 4, !tbaa !9
  %1786 = zext i32 %1785 to i64
  %1787 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1784, i64 noundef %1786) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %161) #10
  %1788 = load double, ptr %142, align 8, !tbaa !15
  %1789 = load double, ptr %140, align 8, !tbaa !15
  %1790 = fmul double %1788, %1789
  %1791 = load double, ptr %158, align 8, !tbaa !15
  %1792 = fmul double %1790, %1791
  %1793 = load double, ptr %156, align 8, !tbaa !15
  %1794 = load double, ptr %141, align 8, !tbaa !15
  %1795 = load double, ptr %139, align 8, !tbaa !15
  %1796 = fmul double %1794, %1795
  %1797 = load double, ptr %157, align 8, !tbaa !15
  %1798 = fmul double %1796, %1797
  %1799 = load double, ptr %156, align 8, !tbaa !15
  %1800 = fmul double %1798, %1799
  %1801 = fneg double %1800
  %1802 = call double @llvm.fmuladd.f64(double %1792, double %1793, double %1801)
  %1803 = load double, ptr %141, align 8, !tbaa !15
  %1804 = load double, ptr %158, align 8, !tbaa !15
  %1805 = fmul double %1803, %1804
  %1806 = load double, ptr %157, align 8, !tbaa !15
  %1807 = fneg double %1805
  %1808 = call double @llvm.fmuladd.f64(double %1807, double %1806, double %1802)
  store double %1808, ptr %161, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %162) #10
  %1809 = load double, ptr %141, align 8, !tbaa !15
  %1810 = fneg double %1809
  %1811 = load double, ptr %140, align 8, !tbaa !15
  %1812 = fmul double %1810, %1811
  %1813 = load double, ptr %158, align 8, !tbaa !15
  %1814 = fmul double %1812, %1813
  %1815 = load double, ptr %156, align 8, !tbaa !15
  %1816 = load double, ptr %142, align 8, !tbaa !15
  %1817 = load double, ptr %139, align 8, !tbaa !15
  %1818 = fmul double %1816, %1817
  %1819 = load double, ptr %157, align 8, !tbaa !15
  %1820 = fmul double %1818, %1819
  %1821 = load double, ptr %156, align 8, !tbaa !15
  %1822 = fmul double %1820, %1821
  %1823 = fneg double %1822
  %1824 = call double @llvm.fmuladd.f64(double %1814, double %1815, double %1823)
  %1825 = load double, ptr %142, align 8, !tbaa !15
  %1826 = load double, ptr %158, align 8, !tbaa !15
  %1827 = fmul double %1825, %1826
  %1828 = load double, ptr %157, align 8, !tbaa !15
  %1829 = fneg double %1827
  %1830 = call double @llvm.fmuladd.f64(double %1829, double %1828, double %1824)
  store double %1830, ptr %162, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %163) #10
  %1831 = load double, ptr %140, align 8, !tbaa !15
  %1832 = fneg double %1831
  %1833 = load double, ptr %157, align 8, !tbaa !15
  %1834 = fmul double %1832, %1833
  %1835 = load double, ptr %156, align 8, !tbaa !15
  %1836 = fmul double %1834, %1835
  store double %1836, ptr %163, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %164) #10
  store double 0.000000e+00, ptr %164, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %165) #10
  store double 0.000000e+00, ptr %165, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %166) #10
  store double 0.000000e+00, ptr %166, align 8, !tbaa !15
  call void @_ZN15SpatialVector_t3setERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %1787, ptr noundef nonnull align 8 dereferenceable(8) %161, ptr noundef nonnull align 8 dereferenceable(8) %162, ptr noundef nonnull align 8 dereferenceable(8) %163, ptr noundef nonnull align 8 dereferenceable(8) %164, ptr noundef nonnull align 8 dereferenceable(8) %165, ptr noundef nonnull align 8 dereferenceable(8) %166)
  call void @llvm.lifetime.end.p0(i64 8, ptr %166) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %165) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %164) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %163) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %162) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %161) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr %167) #10
  %1837 = load ptr, ptr %5, align 8, !tbaa !5
  %1838 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1837, i32 0, i32 15
  %1839 = load i32, ptr %6, align 4, !tbaa !9
  %1840 = zext i32 %1839 to i64
  %1841 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1838, i64 noundef %1840) #10
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %167, ptr noundef nonnull align 8 dereferenceable(96) %143, ptr noundef nonnull align 8 dereferenceable(96) %1841)
  %1842 = load ptr, ptr %5, align 8, !tbaa !5
  %1843 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1842, i32 0, i32 34
  %1844 = load i32, ptr %6, align 4, !tbaa !9
  %1845 = zext i32 %1844 to i64
  %1846 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1843, i64 noundef %1845) #10
  %1847 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %1846, ptr noundef nonnull align 8 dereferenceable(96) %167) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr %167) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %158) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %157) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %156) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr %143) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %142) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %141) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %140) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %139) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %138) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %137) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %136) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %135) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %134) #10
  br label %2315

1848:                                             ; preds = %1580
  %1849 = load ptr, ptr %5, align 8, !tbaa !5
  %1850 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1849, i32 0, i32 10
  %1851 = load i32, ptr %6, align 4, !tbaa !9
  %1852 = zext i32 %1851 to i64
  %1853 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1850, i64 noundef %1852) #10
  %1854 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1853, i32 0, i32 1
  %1855 = load i32, ptr %1854, align 8, !tbaa !11
  %1856 = icmp eq i32 %1855, 10
  br i1 %1856, label %1857, label %2109

1857:                                             ; preds = %1848
  call void @llvm.lifetime.start.p0(i64 8, ptr %168) #10
  %1858 = load ptr, ptr %7, align 8, !tbaa !5
  %1859 = load ptr, ptr %5, align 8, !tbaa !5
  %1860 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1859, i32 0, i32 10
  %1861 = load i32, ptr %6, align 4, !tbaa !9
  %1862 = zext i32 %1861 to i64
  %1863 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1860, i64 noundef %1862) #10
  %1864 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1863, i32 0, i32 3
  %1865 = load i32, ptr %1864, align 8, !tbaa !14
  %1866 = zext i32 %1865 to i64
  %1867 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1858, i64 noundef %1866)
  %1868 = load double, ptr %1867, align 8, !tbaa !15
  store double %1868, ptr %168, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %169) #10
  %1869 = load ptr, ptr %7, align 8, !tbaa !5
  %1870 = load ptr, ptr %5, align 8, !tbaa !5
  %1871 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1870, i32 0, i32 10
  %1872 = load i32, ptr %6, align 4, !tbaa !9
  %1873 = zext i32 %1872 to i64
  %1874 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1871, i64 noundef %1873) #10
  %1875 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1874, i32 0, i32 3
  %1876 = load i32, ptr %1875, align 8, !tbaa !14
  %1877 = add i32 %1876, 1
  %1878 = zext i32 %1877 to i64
  %1879 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1869, i64 noundef %1878)
  %1880 = load double, ptr %1879, align 8, !tbaa !15
  store double %1880, ptr %169, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %170) #10
  %1881 = load ptr, ptr %7, align 8, !tbaa !5
  %1882 = load ptr, ptr %5, align 8, !tbaa !5
  %1883 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1882, i32 0, i32 10
  %1884 = load i32, ptr %6, align 4, !tbaa !9
  %1885 = zext i32 %1884 to i64
  %1886 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1883, i64 noundef %1885) #10
  %1887 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1886, i32 0, i32 3
  %1888 = load i32, ptr %1887, align 8, !tbaa !14
  %1889 = add i32 %1888, 2
  %1890 = zext i32 %1889 to i64
  %1891 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1881, i64 noundef %1890)
  %1892 = load double, ptr %1891, align 8, !tbaa !15
  store double %1892, ptr %170, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %171) #10
  %1893 = load double, ptr %168, align 8, !tbaa !15
  %1894 = call double @sin(double noundef %1893) #10
  store double %1894, ptr %171, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %172) #10
  %1895 = load double, ptr %168, align 8, !tbaa !15
  %1896 = call double @cos(double noundef %1895) #10
  store double %1896, ptr %172, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %173) #10
  %1897 = load double, ptr %169, align 8, !tbaa !15
  %1898 = call double @sin(double noundef %1897) #10
  store double %1898, ptr %173, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %174) #10
  %1899 = load double, ptr %169, align 8, !tbaa !15
  %1900 = call double @cos(double noundef %1899) #10
  store double %1900, ptr %174, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %175) #10
  %1901 = load double, ptr %170, align 8, !tbaa !15
  %1902 = call double @sin(double noundef %1901) #10
  store double %1902, ptr %175, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %176) #10
  %1903 = load double, ptr %170, align 8, !tbaa !15
  %1904 = call double @cos(double noundef %1903) #10
  store double %1904, ptr %176, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 96, ptr %177) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr %178) #10
  call void @llvm.lifetime.start.p0(i64 72, ptr %179) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %180) #10
  %1905 = load double, ptr %171, align 8, !tbaa !15
  %1906 = fneg double %1905
  %1907 = load double, ptr %173, align 8, !tbaa !15
  %1908 = fmul double %1906, %1907
  %1909 = load double, ptr %175, align 8, !tbaa !15
  %1910 = load double, ptr %172, align 8, !tbaa !15
  %1911 = load double, ptr %176, align 8, !tbaa !15
  %1912 = fmul double %1910, %1911
  %1913 = call double @llvm.fmuladd.f64(double %1908, double %1909, double %1912)
  store double %1913, ptr %180, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %181) #10
  %1914 = load double, ptr %171, align 8, !tbaa !15
  %1915 = load double, ptr %176, align 8, !tbaa !15
  %1916 = load double, ptr %173, align 8, !tbaa !15
  %1917 = load double, ptr %175, align 8, !tbaa !15
  %1918 = fmul double %1916, %1917
  %1919 = load double, ptr %172, align 8, !tbaa !15
  %1920 = fmul double %1918, %1919
  %1921 = call double @llvm.fmuladd.f64(double %1914, double %1915, double %1920)
  store double %1921, ptr %181, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %182) #10
  %1922 = load double, ptr %175, align 8, !tbaa !15
  %1923 = fneg double %1922
  %1924 = load double, ptr %174, align 8, !tbaa !15
  %1925 = fmul double %1923, %1924
  store double %1925, ptr %182, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %183) #10
  %1926 = load double, ptr %171, align 8, !tbaa !15
  %1927 = fneg double %1926
  %1928 = load double, ptr %174, align 8, !tbaa !15
  %1929 = fmul double %1927, %1928
  store double %1929, ptr %183, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %184) #10
  %1930 = load double, ptr %172, align 8, !tbaa !15
  %1931 = load double, ptr %174, align 8, !tbaa !15
  %1932 = fmul double %1930, %1931
  store double %1932, ptr %184, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %185) #10
  %1933 = load double, ptr %171, align 8, !tbaa !15
  %1934 = load double, ptr %173, align 8, !tbaa !15
  %1935 = fmul double %1933, %1934
  %1936 = load double, ptr %176, align 8, !tbaa !15
  %1937 = load double, ptr %175, align 8, !tbaa !15
  %1938 = load double, ptr %172, align 8, !tbaa !15
  %1939 = fmul double %1937, %1938
  %1940 = call double @llvm.fmuladd.f64(double %1935, double %1936, double %1939)
  store double %1940, ptr %185, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %186) #10
  %1941 = load double, ptr %171, align 8, !tbaa !15
  %1942 = load double, ptr %175, align 8, !tbaa !15
  %1943 = load double, ptr %173, align 8, !tbaa !15
  %1944 = load double, ptr %172, align 8, !tbaa !15
  %1945 = fmul double %1943, %1944
  %1946 = load double, ptr %176, align 8, !tbaa !15
  %1947 = fmul double %1945, %1946
  %1948 = fneg double %1947
  %1949 = call double @llvm.fmuladd.f64(double %1941, double %1942, double %1948)
  store double %1949, ptr %186, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %187) #10
  %1950 = load double, ptr %174, align 8, !tbaa !15
  %1951 = load double, ptr %176, align 8, !tbaa !15
  %1952 = fmul double %1950, %1951
  store double %1952, ptr %187, align 8, !tbaa !15
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %179, ptr noundef nonnull align 8 dereferenceable(8) %180, ptr noundef nonnull align 8 dereferenceable(8) %181, ptr noundef nonnull align 8 dereferenceable(8) %182, ptr noundef nonnull align 8 dereferenceable(8) %183, ptr noundef nonnull align 8 dereferenceable(8) %184, ptr noundef nonnull align 8 dereferenceable(8) %173, ptr noundef nonnull align 8 dereferenceable(8) %185, ptr noundef nonnull align 8 dereferenceable(8) %186, ptr noundef nonnull align 8 dereferenceable(8) %187)
  call void @llvm.lifetime.start.p0(i64 24, ptr %188) #10
  call void @llvm.lifetime.start.p0(i64 16, ptr %189) #10
  call void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE4ZeroEv(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseNullaryOp.149") align 8 %189)
  call void @_ZN9Vector3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %188, ptr noundef nonnull align 1 dereferenceable(1) %189)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %178, ptr noundef nonnull align 8 dereferenceable(72) %179, ptr noundef nonnull align 8 dereferenceable(24) %188)
  %1953 = load ptr, ptr %5, align 8, !tbaa !5
  %1954 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1953, i32 0, i32 15
  %1955 = load i32, ptr %6, align 4, !tbaa !9
  %1956 = zext i32 %1955 to i64
  %1957 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1954, i64 noundef %1956) #10
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %177, ptr noundef nonnull align 8 dereferenceable(96) %178, ptr noundef nonnull align 8 dereferenceable(96) %1957)
  %1958 = load ptr, ptr %5, align 8, !tbaa !5
  %1959 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1958, i32 0, i32 34
  %1960 = load i32, ptr %6, align 4, !tbaa !9
  %1961 = zext i32 %1960 to i64
  %1962 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1959, i64 noundef %1961) #10
  %1963 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %1962, ptr noundef nonnull align 8 dereferenceable(96) %177) #10
  call void @llvm.lifetime.end.p0(i64 16, ptr %189) #10
  call void @llvm.lifetime.end.p0(i64 24, ptr %188) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %187) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %186) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %185) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %184) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %183) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %182) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %181) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %180) #10
  call void @llvm.lifetime.end.p0(i64 72, ptr %179) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr %178) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr %177) #10
  %1964 = load double, ptr %175, align 8, !tbaa !15
  %1965 = fneg double %1964
  %1966 = load double, ptr %174, align 8, !tbaa !15
  %1967 = fmul double %1965, %1966
  %1968 = load ptr, ptr %5, align 8, !tbaa !5
  %1969 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1968, i32 0, i32 17
  %1970 = load i32, ptr %6, align 4, !tbaa !9
  %1971 = zext i32 %1970 to i64
  %1972 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1969, i64 noundef %1971) #10
  %1973 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1972, i64 noundef 0, i64 noundef 0)
  store double %1967, ptr %1973, align 8, !tbaa !15
  %1974 = load double, ptr %176, align 8, !tbaa !15
  %1975 = load ptr, ptr %5, align 8, !tbaa !5
  %1976 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1975, i32 0, i32 17
  %1977 = load i32, ptr %6, align 4, !tbaa !9
  %1978 = zext i32 %1977 to i64
  %1979 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1976, i64 noundef %1978) #10
  %1980 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1979, i64 noundef 0, i64 noundef 1)
  store double %1974, ptr %1980, align 8, !tbaa !15
  %1981 = load double, ptr %173, align 8, !tbaa !15
  %1982 = load ptr, ptr %5, align 8, !tbaa !5
  %1983 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1982, i32 0, i32 17
  %1984 = load i32, ptr %6, align 4, !tbaa !9
  %1985 = zext i32 %1984 to i64
  %1986 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1983, i64 noundef %1985) #10
  %1987 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1986, i64 noundef 1, i64 noundef 0)
  store double %1981, ptr %1987, align 8, !tbaa !15
  %1988 = load ptr, ptr %5, align 8, !tbaa !5
  %1989 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1988, i32 0, i32 17
  %1990 = load i32, ptr %6, align 4, !tbaa !9
  %1991 = zext i32 %1990 to i64
  %1992 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1989, i64 noundef %1991) #10
  %1993 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1992, i64 noundef 1, i64 noundef 2)
  store double 1.000000e+00, ptr %1993, align 8, !tbaa !15
  %1994 = load double, ptr %174, align 8, !tbaa !15
  %1995 = load double, ptr %176, align 8, !tbaa !15
  %1996 = fmul double %1994, %1995
  %1997 = load ptr, ptr %5, align 8, !tbaa !5
  %1998 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1997, i32 0, i32 17
  %1999 = load i32, ptr %6, align 4, !tbaa !9
  %2000 = zext i32 %1999 to i64
  %2001 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1998, i64 noundef %2000) #10
  %2002 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %2001, i64 noundef 2, i64 noundef 0)
  store double %1996, ptr %2002, align 8, !tbaa !15
  %2003 = load double, ptr %175, align 8, !tbaa !15
  %2004 = load ptr, ptr %5, align 8, !tbaa !5
  %2005 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2004, i32 0, i32 17
  %2006 = load i32, ptr %6, align 4, !tbaa !9
  %2007 = zext i32 %2006 to i64
  %2008 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2005, i64 noundef %2007) #10
  %2009 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %2008, i64 noundef 2, i64 noundef 1)
  store double %2003, ptr %2009, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %190) #10
  %2010 = load ptr, ptr %8, align 8, !tbaa !5
  %2011 = load ptr, ptr %5, align 8, !tbaa !5
  %2012 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2011, i32 0, i32 10
  %2013 = load i32, ptr %6, align 4, !tbaa !9
  %2014 = zext i32 %2013 to i64
  %2015 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2012, i64 noundef %2014) #10
  %2016 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %2015, i32 0, i32 3
  %2017 = load i32, ptr %2016, align 8, !tbaa !14
  %2018 = zext i32 %2017 to i64
  %2019 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2010, i64 noundef %2018)
  %2020 = load double, ptr %2019, align 8, !tbaa !15
  store double %2020, ptr %190, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %191) #10
  %2021 = load ptr, ptr %8, align 8, !tbaa !5
  %2022 = load ptr, ptr %5, align 8, !tbaa !5
  %2023 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2022, i32 0, i32 10
  %2024 = load i32, ptr %6, align 4, !tbaa !9
  %2025 = zext i32 %2024 to i64
  %2026 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2023, i64 noundef %2025) #10
  %2027 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %2026, i32 0, i32 3
  %2028 = load i32, ptr %2027, align 8, !tbaa !14
  %2029 = add i32 %2028, 1
  %2030 = zext i32 %2029 to i64
  %2031 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2021, i64 noundef %2030)
  %2032 = load double, ptr %2031, align 8, !tbaa !15
  store double %2032, ptr %191, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %192) #10
  %2033 = load ptr, ptr %8, align 8, !tbaa !5
  %2034 = load ptr, ptr %5, align 8, !tbaa !5
  %2035 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2034, i32 0, i32 10
  %2036 = load i32, ptr %6, align 4, !tbaa !9
  %2037 = zext i32 %2036 to i64
  %2038 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2035, i64 noundef %2037) #10
  %2039 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %2038, i32 0, i32 3
  %2040 = load i32, ptr %2039, align 8, !tbaa !14
  %2041 = add i32 %2040, 2
  %2042 = zext i32 %2041 to i64
  %2043 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2033, i64 noundef %2042)
  %2044 = load double, ptr %2043, align 8, !tbaa !15
  store double %2044, ptr %192, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 16, ptr %193) #10
  %2045 = load ptr, ptr %5, align 8, !tbaa !5
  %2046 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2045, i32 0, i32 17
  %2047 = load i32, ptr %6, align 4, !tbaa !9
  %2048 = zext i32 %2047 to i64
  %2049 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2046, i64 noundef %2048) #10
  call void @llvm.lifetime.start.p0(i64 24, ptr %194) #10
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %194, ptr noundef nonnull align 8 dereferenceable(8) %190, ptr noundef nonnull align 8 dereferenceable(8) %191, ptr noundef nonnull align 8 dereferenceable(8) %192)
  %2050 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEEEmlINS1_IdLi3ELi1ELi0ELi3ELi1EEEEEKNS_7ProductIS2_T_Li0EEERKNS0_IS7_EE(ptr noundef nonnull align 1 dereferenceable(1) %2049, ptr noundef nonnull align 1 dereferenceable(1) %194)
  %2051 = getelementptr inbounds { ptr, ptr }, ptr %193, i32 0, i32 0
  %2052 = extractvalue { ptr, ptr } %2050, 0
  store ptr %2052, ptr %2051, align 8
  %2053 = getelementptr inbounds { ptr, ptr }, ptr %193, i32 0, i32 1
  %2054 = extractvalue { ptr, ptr } %2050, 1
  store ptr %2054, ptr %2053, align 8
  %2055 = load ptr, ptr %5, align 8, !tbaa !5
  %2056 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2055, i32 0, i32 12
  %2057 = load i32, ptr %6, align 4, !tbaa !9
  %2058 = zext i32 %2057 to i64
  %2059 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2056, i64 noundef %2058) #10
  %2060 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen7ProductINS1_6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS3_IdLi3ELi1ELi0ELi3ELi1EEELi0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48) %2059, ptr noundef nonnull align 1 dereferenceable(1) %193)
  call void @llvm.lifetime.end.p0(i64 24, ptr %194) #10
  call void @llvm.lifetime.end.p0(i64 16, ptr %193) #10
  %2061 = load ptr, ptr %5, align 8, !tbaa !5
  %2062 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2061, i32 0, i32 13
  %2063 = load i32, ptr %6, align 4, !tbaa !9
  %2064 = zext i32 %2063 to i64
  %2065 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2062, i64 noundef %2064) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %195) #10
  %2066 = load double, ptr %174, align 8, !tbaa !15
  %2067 = fneg double %2066
  %2068 = load double, ptr %176, align 8, !tbaa !15
  %2069 = fmul double %2067, %2068
  %2070 = load double, ptr %192, align 8, !tbaa !15
  %2071 = load double, ptr %173, align 8, !tbaa !15
  %2072 = load double, ptr %175, align 8, !tbaa !15
  %2073 = fmul double %2071, %2072
  %2074 = load double, ptr %191, align 8, !tbaa !15
  %2075 = fmul double %2073, %2074
  %2076 = call double @llvm.fmuladd.f64(double %2069, double %2070, double %2075)
  %2077 = load double, ptr %190, align 8, !tbaa !15
  %2078 = load double, ptr %175, align 8, !tbaa !15
  %2079 = load double, ptr %191, align 8, !tbaa !15
  %2080 = fmul double %2078, %2079
  %2081 = load double, ptr %192, align 8, !tbaa !15
  %2082 = fmul double %2080, %2081
  %2083 = fneg double %2082
  %2084 = call double @llvm.fmuladd.f64(double %2076, double %2077, double %2083)
  store double %2084, ptr %195, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %196) #10
  %2085 = load double, ptr %174, align 8, !tbaa !15
  %2086 = load double, ptr %191, align 8, !tbaa !15
  %2087 = fmul double %2085, %2086
  %2088 = load double, ptr %190, align 8, !tbaa !15
  %2089 = fmul double %2087, %2088
  store double %2089, ptr %196, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %197) #10
  %2090 = load double, ptr %173, align 8, !tbaa !15
  %2091 = fneg double %2090
  %2092 = load double, ptr %176, align 8, !tbaa !15
  %2093 = fmul double %2091, %2092
  %2094 = load double, ptr %191, align 8, !tbaa !15
  %2095 = load double, ptr %174, align 8, !tbaa !15
  %2096 = load double, ptr %175, align 8, !tbaa !15
  %2097 = fmul double %2095, %2096
  %2098 = load double, ptr %192, align 8, !tbaa !15
  %2099 = fmul double %2097, %2098
  %2100 = fneg double %2099
  %2101 = call double @llvm.fmuladd.f64(double %2093, double %2094, double %2100)
  %2102 = load double, ptr %190, align 8, !tbaa !15
  %2103 = load double, ptr %176, align 8, !tbaa !15
  %2104 = load double, ptr %192, align 8, !tbaa !15
  %2105 = fmul double %2103, %2104
  %2106 = load double, ptr %191, align 8, !tbaa !15
  %2107 = fmul double %2105, %2106
  %2108 = call double @llvm.fmuladd.f64(double %2101, double %2102, double %2107)
  store double %2108, ptr %197, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %198) #10
  store double 0.000000e+00, ptr %198, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %199) #10
  store double 0.000000e+00, ptr %199, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %200) #10
  store double 0.000000e+00, ptr %200, align 8, !tbaa !15
  call void @_ZN15SpatialVector_t3setERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %2065, ptr noundef nonnull align 8 dereferenceable(8) %195, ptr noundef nonnull align 8 dereferenceable(8) %196, ptr noundef nonnull align 8 dereferenceable(8) %197, ptr noundef nonnull align 8 dereferenceable(8) %198, ptr noundef nonnull align 8 dereferenceable(8) %199, ptr noundef nonnull align 8 dereferenceable(8) %200)
  call void @llvm.lifetime.end.p0(i64 8, ptr %200) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %199) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %198) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %197) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %196) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %195) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %192) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %191) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %190) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %176) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %175) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %174) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %173) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %172) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %171) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %170) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %169) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %168) #10
  br label %2314

2109:                                             ; preds = %1848
  %2110 = load ptr, ptr %5, align 8, !tbaa !5
  %2111 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2110, i32 0, i32 10
  %2112 = load i32, ptr %6, align 4, !tbaa !9
  %2113 = zext i32 %2112 to i64
  %2114 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2111, i64 noundef %2113) #10
  %2115 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %2114, i32 0, i32 1
  %2116 = load i32, ptr %2115, align 8, !tbaa !11
  %2117 = icmp eq i32 %2116, 11
  br i1 %2117, label %2118, label %2267

2118:                                             ; preds = %2109
  call void @llvm.lifetime.start.p0(i64 8, ptr %201) #10
  %2119 = load ptr, ptr %7, align 8, !tbaa !5
  %2120 = load ptr, ptr %5, align 8, !tbaa !5
  %2121 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2120, i32 0, i32 10
  %2122 = load i32, ptr %6, align 4, !tbaa !9
  %2123 = zext i32 %2122 to i64
  %2124 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2121, i64 noundef %2123) #10
  %2125 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %2124, i32 0, i32 3
  %2126 = load i32, ptr %2125, align 8, !tbaa !14
  %2127 = zext i32 %2126 to i64
  %2128 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2119, i64 noundef %2127)
  %2129 = load double, ptr %2128, align 8, !tbaa !15
  store double %2129, ptr %201, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %202) #10
  %2130 = load ptr, ptr %7, align 8, !tbaa !5
  %2131 = load ptr, ptr %5, align 8, !tbaa !5
  %2132 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2131, i32 0, i32 10
  %2133 = load i32, ptr %6, align 4, !tbaa !9
  %2134 = zext i32 %2133 to i64
  %2135 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2132, i64 noundef %2134) #10
  %2136 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %2135, i32 0, i32 3
  %2137 = load i32, ptr %2136, align 8, !tbaa !14
  %2138 = add i32 %2137, 1
  %2139 = zext i32 %2138 to i64
  %2140 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2130, i64 noundef %2139)
  %2141 = load double, ptr %2140, align 8, !tbaa !15
  store double %2141, ptr %202, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %203) #10
  %2142 = load ptr, ptr %7, align 8, !tbaa !5
  %2143 = load ptr, ptr %5, align 8, !tbaa !5
  %2144 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2143, i32 0, i32 10
  %2145 = load i32, ptr %6, align 4, !tbaa !9
  %2146 = zext i32 %2145 to i64
  %2147 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2144, i64 noundef %2146) #10
  %2148 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %2147, i32 0, i32 3
  %2149 = load i32, ptr %2148, align 8, !tbaa !14
  %2150 = add i32 %2149, 2
  %2151 = zext i32 %2150 to i64
  %2152 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2142, i64 noundef %2151)
  %2153 = load double, ptr %2152, align 8, !tbaa !15
  store double %2153, ptr %203, align 8, !tbaa !15
  %2154 = load ptr, ptr %5, align 8, !tbaa !5
  %2155 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2154, i32 0, i32 17
  %2156 = load i32, ptr %6, align 4, !tbaa !9
  %2157 = zext i32 %2156 to i64
  %2158 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2155, i64 noundef %2157) #10
  %2159 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %2158, i64 noundef 3, i64 noundef 0)
  store double 1.000000e+00, ptr %2159, align 8, !tbaa !15
  %2160 = load ptr, ptr %5, align 8, !tbaa !5
  %2161 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2160, i32 0, i32 17
  %2162 = load i32, ptr %6, align 4, !tbaa !9
  %2163 = zext i32 %2162 to i64
  %2164 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2161, i64 noundef %2163) #10
  %2165 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %2164, i64 noundef 4, i64 noundef 1)
  store double 1.000000e+00, ptr %2165, align 8, !tbaa !15
  %2166 = load ptr, ptr %5, align 8, !tbaa !5
  %2167 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2166, i32 0, i32 17
  %2168 = load i32, ptr %6, align 4, !tbaa !9
  %2169 = zext i32 %2168 to i64
  %2170 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2167, i64 noundef %2169) #10
  %2171 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %2170, i64 noundef 5, i64 noundef 2)
  store double 1.000000e+00, ptr %2171, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %204) #10
  %2172 = load ptr, ptr %8, align 8, !tbaa !5
  %2173 = load ptr, ptr %5, align 8, !tbaa !5
  %2174 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2173, i32 0, i32 10
  %2175 = load i32, ptr %6, align 4, !tbaa !9
  %2176 = zext i32 %2175 to i64
  %2177 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2174, i64 noundef %2176) #10
  %2178 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %2177, i32 0, i32 3
  %2179 = load i32, ptr %2178, align 8, !tbaa !14
  %2180 = zext i32 %2179 to i64
  %2181 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2172, i64 noundef %2180)
  %2182 = load double, ptr %2181, align 8, !tbaa !15
  store double %2182, ptr %204, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %205) #10
  %2183 = load ptr, ptr %8, align 8, !tbaa !5
  %2184 = load ptr, ptr %5, align 8, !tbaa !5
  %2185 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2184, i32 0, i32 10
  %2186 = load i32, ptr %6, align 4, !tbaa !9
  %2187 = zext i32 %2186 to i64
  %2188 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2185, i64 noundef %2187) #10
  %2189 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %2188, i32 0, i32 3
  %2190 = load i32, ptr %2189, align 8, !tbaa !14
  %2191 = add i32 %2190, 1
  %2192 = zext i32 %2191 to i64
  %2193 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2183, i64 noundef %2192)
  %2194 = load double, ptr %2193, align 8, !tbaa !15
  store double %2194, ptr %205, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %206) #10
  %2195 = load ptr, ptr %8, align 8, !tbaa !5
  %2196 = load ptr, ptr %5, align 8, !tbaa !5
  %2197 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2196, i32 0, i32 10
  %2198 = load i32, ptr %6, align 4, !tbaa !9
  %2199 = zext i32 %2198 to i64
  %2200 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2197, i64 noundef %2199) #10
  %2201 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %2200, i32 0, i32 3
  %2202 = load i32, ptr %2201, align 8, !tbaa !14
  %2203 = add i32 %2202, 2
  %2204 = zext i32 %2203 to i64
  %2205 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2195, i64 noundef %2204)
  %2206 = load double, ptr %2205, align 8, !tbaa !15
  store double %2206, ptr %206, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 16, ptr %207) #10
  %2207 = load ptr, ptr %5, align 8, !tbaa !5
  %2208 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2207, i32 0, i32 17
  %2209 = load i32, ptr %6, align 4, !tbaa !9
  %2210 = zext i32 %2209 to i64
  %2211 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2208, i64 noundef %2210) #10
  call void @llvm.lifetime.start.p0(i64 24, ptr %208) #10
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %208, ptr noundef nonnull align 8 dereferenceable(8) %204, ptr noundef nonnull align 8 dereferenceable(8) %205, ptr noundef nonnull align 8 dereferenceable(8) %206)
  %2212 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEEEmlINS1_IdLi3ELi1ELi0ELi3ELi1EEEEEKNS_7ProductIS2_T_Li0EEERKNS0_IS7_EE(ptr noundef nonnull align 1 dereferenceable(1) %2211, ptr noundef nonnull align 1 dereferenceable(1) %208)
  %2213 = getelementptr inbounds { ptr, ptr }, ptr %207, i32 0, i32 0
  %2214 = extractvalue { ptr, ptr } %2212, 0
  store ptr %2214, ptr %2213, align 8
  %2215 = getelementptr inbounds { ptr, ptr }, ptr %207, i32 0, i32 1
  %2216 = extractvalue { ptr, ptr } %2212, 1
  store ptr %2216, ptr %2215, align 8
  %2217 = load ptr, ptr %5, align 8, !tbaa !5
  %2218 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2217, i32 0, i32 12
  %2219 = load i32, ptr %6, align 4, !tbaa !9
  %2220 = zext i32 %2219 to i64
  %2221 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2218, i64 noundef %2220) #10
  %2222 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen7ProductINS1_6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS3_IdLi3ELi1ELi0ELi3ELi1EEELi0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48) %2221, ptr noundef nonnull align 1 dereferenceable(1) %207)
  call void @llvm.lifetime.end.p0(i64 24, ptr %208) #10
  call void @llvm.lifetime.end.p0(i64 16, ptr %207) #10
  %2223 = load ptr, ptr %5, align 8, !tbaa !5
  %2224 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2223, i32 0, i32 13
  %2225 = load i32, ptr %6, align 4, !tbaa !9
  %2226 = zext i32 %2225 to i64
  %2227 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2224, i64 noundef %2226) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %209) #10
  store double 0.000000e+00, ptr %209, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %210) #10
  store double 0.000000e+00, ptr %210, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %211) #10
  store double 0.000000e+00, ptr %211, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %212) #10
  store double 0.000000e+00, ptr %212, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %213) #10
  store double 0.000000e+00, ptr %213, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %214) #10
  store double 0.000000e+00, ptr %214, align 8, !tbaa !15
  call void @_ZN15SpatialVector_t3setERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %2227, ptr noundef nonnull align 8 dereferenceable(8) %209, ptr noundef nonnull align 8 dereferenceable(8) %210, ptr noundef nonnull align 8 dereferenceable(8) %211, ptr noundef nonnull align 8 dereferenceable(8) %212, ptr noundef nonnull align 8 dereferenceable(8) %213, ptr noundef nonnull align 8 dereferenceable(8) %214)
  call void @llvm.lifetime.end.p0(i64 8, ptr %214) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %213) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %212) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %211) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %210) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %209) #10
  %2228 = load ptr, ptr %5, align 8, !tbaa !5
  %2229 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2228, i32 0, i32 15
  %2230 = load i32, ptr %6, align 4, !tbaa !9
  %2231 = zext i32 %2230 to i64
  %2232 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2229, i64 noundef %2231) #10
  %2233 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %2232, i32 0, i32 0
  %2234 = load ptr, ptr %5, align 8, !tbaa !5
  %2235 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2234, i32 0, i32 34
  %2236 = load i32, ptr %6, align 4, !tbaa !9
  %2237 = zext i32 %2236 to i64
  %2238 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2235, i64 noundef %2237) #10
  %2239 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %2238, i32 0, i32 0
  %2240 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSERKS_(ptr noundef nonnull align 8 dereferenceable(72) %2239, ptr noundef nonnull align 8 dereferenceable(72) %2233)
  call void @llvm.lifetime.start.p0(i64 32, ptr %215) #10
  %2241 = load ptr, ptr %5, align 8, !tbaa !5
  %2242 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2241, i32 0, i32 15
  %2243 = load i32, ptr %6, align 4, !tbaa !9
  %2244 = zext i32 %2243 to i64
  %2245 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2242, i64 noundef %2244) #10
  %2246 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %2245, i32 0, i32 1
  call void @llvm.lifetime.start.p0(i64 16, ptr %216) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr %217) #10
  %2247 = load ptr, ptr %5, align 8, !tbaa !5
  %2248 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2247, i32 0, i32 15
  %2249 = load i32, ptr %6, align 4, !tbaa !9
  %2250 = zext i32 %2249 to i64
  %2251 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2248, i64 noundef %2250) #10
  %2252 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %2251, i32 0, i32 0
  %2253 = call ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1) %2252)
  %2254 = getelementptr inbounds %"class.Eigen::Transpose", ptr %217, i32 0, i32 0
  store ptr %2253, ptr %2254, align 8
  call void @llvm.lifetime.start.p0(i64 24, ptr %218) #10
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %218, ptr noundef nonnull align 8 dereferenceable(8) %201, ptr noundef nonnull align 8 dereferenceable(8) %202, ptr noundef nonnull align 8 dereferenceable(8) %203)
  %2255 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_9TransposeINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEmlINS2_IdLi3ELi1ELi0ELi3ELi1EEEEEKNS_7ProductIS4_T_Li0EEERKNS0_IS9_EE(ptr noundef nonnull align 1 dereferenceable(1) %217, ptr noundef nonnull align 1 dereferenceable(1) %218)
  %2256 = getelementptr inbounds { ptr, ptr }, ptr %216, i32 0, i32 0
  %2257 = extractvalue { ptr, ptr } %2255, 0
  store ptr %2257, ptr %2256, align 8
  %2258 = getelementptr inbounds { ptr, ptr }, ptr %216, i32 0, i32 1
  %2259 = extractvalue { ptr, ptr } %2255, 1
  store ptr %2259, ptr %2258, align 8
  call void @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEplINS_7ProductINS_9TransposeINS1_IdLi3ELi3ELi0ELi3ELi3EEEEES2_Li0EEEEEKNS_13CwiseBinaryOpINS_8internal13scalar_sum_opIdNSB_6traitsIT_E6ScalarEEEKS2_KSE_EERKNS0_ISE_EE(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseBinaryOp.161") align 8 %215, ptr noundef nonnull align 1 dereferenceable(1) %2246, ptr noundef nonnull align 1 dereferenceable(1) %216)
  %2260 = load ptr, ptr %5, align 8, !tbaa !5
  %2261 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2260, i32 0, i32 34
  %2262 = load i32, ptr %6, align 4, !tbaa !9
  %2263 = zext i32 %2262 to i64
  %2264 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2261, i64 noundef %2263) #10
  %2265 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %2264, i32 0, i32 1
  %2266 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSIN5Eigen13CwiseBinaryOpINS1_8internal13scalar_sum_opIddEEKNS1_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEKNS1_7ProductINS1_9TransposeINS6_IdLi3ELi3ELi0ELi3ELi3EEEEES7_Li0EEEEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %2265, ptr noundef nonnull align 1 dereferenceable(1) %215)
  call void @llvm.lifetime.end.p0(i64 24, ptr %218) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %217) #10
  call void @llvm.lifetime.end.p0(i64 16, ptr %216) #10
  call void @llvm.lifetime.end.p0(i64 32, ptr %215) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %206) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %205) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %204) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %203) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %202) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %201) #10
  br label %2313

2267:                                             ; preds = %2109
  %2268 = load ptr, ptr %5, align 8, !tbaa !5
  %2269 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2268, i32 0, i32 10
  %2270 = load i32, ptr %6, align 4, !tbaa !9
  %2271 = zext i32 %2270 to i64
  %2272 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2269, i64 noundef %2271) #10
  %2273 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %2272, i32 0, i32 1
  %2274 = load i32, ptr %2273, align 8, !tbaa !11
  %2275 = icmp eq i32 %2274, 21
  br i1 %2275, label %2276, label %2298

2276:                                             ; preds = %2267
  call void @llvm.lifetime.start.p0(i64 8, ptr %219) #10
  %2277 = load ptr, ptr %5, align 8, !tbaa !5
  %2278 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2277, i32 0, i32 10
  %2279 = load i32, ptr %6, align 4, !tbaa !9
  %2280 = zext i32 %2279 to i64
  %2281 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2278, i64 noundef %2280) #10
  store ptr %2281, ptr %219, align 8, !tbaa !5
  call void @llvm.lifetime.start.p0(i64 8, ptr %220) #10
  %2282 = load ptr, ptr %5, align 8, !tbaa !5
  %2283 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2282, i32 0, i32 22
  %2284 = load ptr, ptr %219, align 8, !tbaa !5
  %2285 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %2284, i32 0, i32 4
  %2286 = load i32, ptr %2285, align 4, !tbaa !19
  %2287 = sext i32 %2286 to i64
  %2288 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSt6vectorIPN17RigidBodyDynamics11CustomJointESaIS2_EEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2283, i64 noundef %2287) #10
  %2289 = load ptr, ptr %2288, align 8, !tbaa !5
  store ptr %2289, ptr %220, align 8, !tbaa !5
  %2290 = load ptr, ptr %220, align 8, !tbaa !5
  %2291 = load ptr, ptr %5, align 8, !tbaa !5
  %2292 = load i32, ptr %6, align 4, !tbaa !9
  %2293 = load ptr, ptr %7, align 8, !tbaa !5
  %2294 = load ptr, ptr %8, align 8, !tbaa !5
  %2295 = load ptr, ptr %2290, align 8, !tbaa !20
  %2296 = getelementptr inbounds ptr, ptr %2295, i64 2
  %2297 = load ptr, ptr %2296, align 8
  call void %2297(ptr noundef nonnull align 8 dereferenceable(216) %2290, ptr noundef nonnull align 8 dereferenceable(872) %2291, i32 noundef %2292, ptr noundef nonnull align 8 dereferenceable(16) %2293, ptr noundef nonnull align 8 dereferenceable(16) %2294)
  call void @llvm.lifetime.end.p0(i64 8, ptr %220) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr %219) #10
  br label %2312

2298:                                             ; preds = %2267
  %2299 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cerr, ptr noundef @.str.5)
  %2300 = load ptr, ptr %5, align 8, !tbaa !5
  %2301 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %2300, i32 0, i32 10
  %2302 = load i32, ptr %6, align 4, !tbaa !9
  %2303 = zext i32 %2302 to i64
  %2304 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %2301, i64 noundef %2303) #10
  %2305 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %2304, i32 0, i32 1
  %2306 = load i32, ptr %2305, align 8, !tbaa !11
  %2307 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEi(ptr noundef nonnull align 8 dereferenceable(8) %2299, i32 noundef %2306)
  %2308 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) %2307, ptr noundef @.str.6)
  %2309 = load i32, ptr %6, align 4, !tbaa !9
  %2310 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEj(ptr noundef nonnull align 8 dereferenceable(8) %2308, i32 noundef %2309)
  %2311 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %2310, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
  call void @abort() #9
  unreachable

2312:                                             ; preds = %2276
  br label %2313

2313:                                             ; preds = %2312, %2118
  br label %2314

2314:                                             ; preds = %2313, %1857
  br label %2315

2315:                                             ; preds = %2314, %1589
  br label %2316

2316:                                             ; preds = %2315, %1324
  br label %2317

2317:                                             ; preds = %2316, %1056
  br label %2318

2318:                                             ; preds = %2317, %973
  br label %2319

2319:                                             ; preds = %2318, %928
  br label %2320

2320:                                             ; preds = %2319, %841
  br label %2321

2321:                                             ; preds = %2320, %639
  br label %2322

2322:                                             ; preds = %2321, %437
  br label %2323

2323:                                             ; preds = %2322, %235
  ret void
}

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

; 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) #2 align 2

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

; Function Attrs: nounwind
declare void @sincos(double noundef, ptr noundef, ptr noundef) #4

; 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) #5 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) #2 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) #5 align 2

; Function Attrs: nocallback nofree nosync nounwind speculatable willreturn memory(none)
declare double @llvm.fmuladd.f64(double, double, double) #6

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

; Function Attrs: inlinehint mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(72), ptr noundef nonnull align 8 dereferenceable(72)) #7 align 2

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

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSERKS_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(24)) #5 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) #2 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) #5 align 2

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

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

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

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen13CwiseBinaryOpINS1_8internal17scalar_product_opIddEEKNS1_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEKNS1_14CwiseNullaryOpINS3_18scalar_constant_opIdEES8_EEEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 1 dereferenceable(1)) #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEE5blockIiiEENS3_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, i32 noundef) #5 align 2

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

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

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

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

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

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

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

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

; Function Attrs: inlinehint mustprogress nounwind uwtable
declare dso_local noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSEOS_(ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 16 dereferenceable(48)) #7 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind noalias writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8, ptr noundef nonnull align 8 dereferenceable(96), ptr noundef nonnull align 8 dereferenceable(96)) #0 align 2

; Function Attrs: inlinehint mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96), ptr noundef nonnull align 8 dereferenceable(96)) #7 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)) #0 align 2

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

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

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96), ptr noundef nonnull align 8 dereferenceable(72), ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #0 align 2

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

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

; Function Attrs: nounwind
declare double @sin(double noundef) #4

; Function Attrs: nounwind
declare double @cos(double noundef) #4

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE4ZeroEv(ptr dead_on_unwind noalias writable sret(%"class.Eigen::CwiseNullaryOp.149") align 8) #5 align 2

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

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

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen7ProductINS1_6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS3_IdLi3ELi1ELi0ELi3ELi1EEELi0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 1 dereferenceable(1)) #0 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN15SpatialVector_t3setERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8)) #0 align 2

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

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

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

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

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

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZNSt6vectorIPN17RigidBodyDynamics11CustomJointESaIS2_EEixEm(ptr noundef nonnull align 8 dereferenceable(24), i64 noundef) #2 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) #5

declare noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEi(ptr noundef nonnull align 8 dereferenceable(8), i32 noundef) #8

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

; Function Attrs: mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8), ptr noundef) #0 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)) #5

; Function Attrs: noreturn nounwind
declare void @abort() #1

attributes #0 = { 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 #1 = { 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 #2 = { 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 #3 = { nocallback nofree nosync nounwind willreturn memory(argmem: readwrite) }
attributes #4 = { 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 #5 = { 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 #6 = { nocallback nofree nosync nounwind speculatable willreturn memory(none) }
attributes #7 = { 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 #8 = { "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 #9 = { noreturn nounwind }
attributes #10 = { 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 = !{!"int", !7, i64 0}
!11 = !{!12, !13, i64 8}
!12 = !{!"_ZTSN17RigidBodyDynamics5JointE", !6, i64 0, !13, i64 8, !10, i64 12, !10, i64 16, !10, i64 20}
!13 = !{!"_ZTSN17RigidBodyDynamics9JointTypeE", !7, i64 0}
!14 = !{!12, !10, i64 16}
!15 = !{!16, !16, i64 0}
!16 = !{!"double", !7, i64 0}
!17 = !{!12, !6, i64 0}
!18 = !{!12, !10, i64 12}
!19 = !{!12, !10, i64 20}
!20 = !{!21, !21, i64 0}
!21 = !{!"vtable pointer", !8, i64 0}


; 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.Matrix3_t = type { %"class.Eigen::Matrix.90" }
%"class.Eigen::Matrix.90" = type { %"class.Eigen::PlainObjectBase.91" }
%"class.Eigen::PlainObjectBase.91" = type { %"class.Eigen::DenseStorage.98" }
%"class.Eigen::DenseStorage.98" = type { %"struct.Eigen::internal::plain_array.99" }
%"struct.Eigen::internal::plain_array.99" = type { [9 x double] }
%"struct.RigidBodyDynamics::Math::SpatialTransform" = type { %class.Matrix3_t, %class.Vector3_t }
%class.Vector3_t = type { %"class.Eigen::Matrix" }
%"class.Eigen::Matrix" = type { %"class.Eigen::PlainObjectBase" }
%"class.Eigen::PlainObjectBase" = type { %"class.Eigen::DenseStorage" }
%"class.Eigen::DenseStorage" = type { %"struct.Eigen::internal::plain_array" }
%"struct.Eigen::internal::plain_array" = type { [3 x double] }
%"class.Eigen::CwiseBinaryOp" = type <{ ptr, %"class.Eigen::CwiseNullaryOp", [8 x i8] }>
%"class.Eigen::CwiseNullaryOp" = type { [8 x i8], %"struct.Eigen::internal::scalar_constant_op" }
%"struct.Eigen::internal::scalar_constant_op" = type { double }
%"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.118" }
%"class.Eigen::MapBase.118" = type { ptr, %"class.Eigen::internal::variable_if_dynamic", %"class.Eigen::internal::variable_if_dynamic" }
%"class.Eigen::internal::variable_if_dynamic" = type { i64 }
%"class.Eigen::Product" = type { ptr, %"class.Eigen::Block" }
%class.SpatialVector_t = type { %"class.Eigen::Matrix.100" }
%"class.Eigen::Matrix.100" = type { %"class.Eigen::PlainObjectBase.101" }
%"class.Eigen::PlainObjectBase.101" = type { %"class.Eigen::DenseStorage.108" }
%"class.Eigen::DenseStorage.108" = type { %"struct.Eigen::internal::plain_array.109" }
%"struct.Eigen::internal::plain_array.109" = type { [6 x double] }
%"class.RigidBodyDynamics::Math::Quaternion" = type { %class.Vector4_t }
%class.Vector4_t = type { %"class.Eigen::Matrix.129" }
%"class.Eigen::Matrix.129" = type { %"class.Eigen::PlainObjectBase.130" }
%"class.Eigen::PlainObjectBase.130" = type { %"class.Eigen::DenseStorage.137" }
%"class.Eigen::DenseStorage.137" = type { %"struct.Eigen::internal::plain_array.138" }
%"struct.Eigen::internal::plain_array.138" = type { [4 x double] }
%"class.Eigen::CwiseNullaryOp.149" = type { [8 x i8], %"struct.Eigen::internal::scalar_constant_op" }
%"class.Eigen::Product.154" = type { ptr, ptr }
%"class.Eigen::CwiseBinaryOp.161" = type <{ ptr, %"class.Eigen::Product.167", [8 x i8] }>
%"class.Eigen::Product.167" = type { %"class.Eigen::Transpose", ptr }
%"class.Eigen::Transpose" = type { ptr }

@.str = external hidden unnamed_addr constant [13 x i8], align 1
@.str.4 = external hidden unnamed_addr constant [106 x i8], align 1
@__PRETTY_FUNCTION__._ZN17RigidBodyDynamics5jcalcERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEES6_ = external hidden unnamed_addr constant [89 x i8], align 1
@_ZSt4cerr = external global %"class.std::basic_ostream", align 8
@.str.5 = external hidden unnamed_addr constant [27 x i8], align 1
@.str.6 = external hidden unnamed_addr constant [8 x i8], align 1

; Function Attrs: mustprogress uwtable
define dso_local void @_ZN17RigidBodyDynamics5jcalcERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEES6_(ptr noundef nonnull align 8 dereferenceable(872) %0, i32 noundef %1, ptr noundef nonnull align 8 dereferenceable(16) %2, ptr noundef nonnull align 8 dereferenceable(16) %3) local_unnamed_addr #0 {
  %5 = alloca double, align 8
  %6 = alloca double, align 8
  %7 = alloca %class.Matrix3_t, align 8
  %8 = alloca double, align 8
  %9 = alloca double, align 8
  %10 = alloca double, align 8
  %11 = alloca double, align 8
  %12 = alloca double, align 8
  %13 = alloca double, align 8
  %14 = alloca double, align 8
  %15 = alloca double, align 8
  %16 = alloca %class.Matrix3_t, align 8
  %17 = alloca double, align 8
  %18 = alloca double, align 8
  %19 = alloca double, align 8
  %20 = alloca double, align 8
  %21 = alloca double, align 8
  %22 = alloca double, align 8
  %23 = alloca double, align 8
  %24 = alloca double, align 8
  %25 = alloca %class.Matrix3_t, align 8
  %26 = alloca double, align 8
  %27 = alloca double, align 8
  %28 = alloca double, align 8
  %29 = alloca double, align 8
  %30 = alloca double, align 8
  %31 = alloca double, align 8
  %32 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %33 = alloca double, align 8
  %34 = alloca %"class.Eigen::CwiseBinaryOp", align 8
  %35 = alloca %class.Vector3_t, align 8
  %36 = alloca %"class.Eigen::Block", align 8
  %37 = alloca %class.Vector3_t, align 8
  %38 = alloca %"class.Eigen::Product", align 8
  %39 = alloca %"class.Eigen::Block", align 8
  %40 = alloca %"class.Eigen::Matrix", align 8
  %41 = alloca double, align 8
  %42 = alloca %class.SpatialVector_t, align 16
  %43 = alloca double, align 8
  %44 = alloca double, align 8
  %45 = alloca double, align 8
  %46 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %47 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %48 = alloca %"class.Eigen::CwiseBinaryOp", align 8
  %49 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %50 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %51 = alloca %class.Matrix3_t, align 8
  %52 = alloca %"class.RigidBodyDynamics::Math::Quaternion", align 16
  %53 = alloca %class.Vector3_t, align 8
  %54 = alloca double, align 8
  %55 = alloca double, align 8
  %56 = alloca double, align 8
  %57 = alloca %class.Vector3_t, align 8
  %58 = alloca %class.SpatialVector_t, align 16
  %59 = alloca double, align 8
  %60 = alloca double, align 8
  %61 = alloca double, align 8
  %62 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %63 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %64 = alloca %class.Matrix3_t, align 8
  %65 = alloca double, align 8
  %66 = alloca double, align 8
  %67 = alloca double, align 8
  %68 = alloca double, align 8
  %69 = alloca double, align 8
  %70 = alloca double, align 8
  %71 = alloca double, align 8
  %72 = alloca double, align 8
  %73 = alloca double, align 8
  %74 = alloca %class.Vector3_t, align 8
  %75 = alloca %"class.Eigen::CwiseNullaryOp.149", align 8
  %76 = alloca double, align 8
  %77 = alloca double, align 8
  %78 = alloca double, align 8
  %79 = alloca %"class.Eigen::Product.154", align 8
  %80 = alloca %class.Vector3_t, align 8
  %81 = alloca double, align 8
  %82 = alloca double, align 8
  %83 = alloca double, align 8
  %84 = alloca double, align 8
  %85 = alloca double, align 8
  %86 = alloca double, align 8
  %87 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %88 = alloca double, align 8
  %89 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %90 = alloca %class.Matrix3_t, align 8
  %91 = alloca double, align 8
  %92 = alloca double, align 8
  %93 = alloca double, align 8
  %94 = alloca double, align 8
  %95 = alloca double, align 8
  %96 = alloca double, align 8
  %97 = alloca double, align 8
  %98 = alloca double, align 8
  %99 = alloca %class.Vector3_t, align 8
  %100 = alloca %"class.Eigen::CwiseNullaryOp.149", align 8
  %101 = alloca double, align 8
  %102 = alloca double, align 8
  %103 = alloca double, align 8
  %104 = alloca %"class.Eigen::Product.154", align 8
  %105 = alloca %class.Vector3_t, align 8
  %106 = alloca double, align 8
  %107 = alloca double, align 8
  %108 = alloca double, align 8
  %109 = alloca double, align 8
  %110 = alloca double, align 8
  %111 = alloca double, align 8
  %112 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %113 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %114 = alloca %class.Matrix3_t, align 8
  %115 = alloca double, align 8
  %116 = alloca double, align 8
  %117 = alloca double, align 8
  %118 = alloca double, align 8
  %119 = alloca double, align 8
  %120 = alloca double, align 8
  %121 = alloca double, align 8
  %122 = alloca double, align 8
  %123 = alloca double, align 8
  %124 = alloca %class.Vector3_t, align 8
  %125 = alloca %"class.Eigen::CwiseNullaryOp.149", align 8
  %126 = alloca double, align 8
  %127 = alloca double, align 8
  %128 = alloca double, align 8
  %129 = alloca %"class.Eigen::Product.154", align 8
  %130 = alloca %class.Vector3_t, align 8
  %131 = alloca double, align 8
  %132 = alloca double, align 8
  %133 = alloca double, align 8
  %134 = alloca double, align 8
  %135 = alloca double, align 8
  %136 = alloca double, align 8
  %137 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %138 = alloca double, align 8
  %139 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %140 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %141 = alloca %class.Matrix3_t, align 8
  %142 = alloca double, align 8
  %143 = alloca double, align 8
  %144 = alloca double, align 8
  %145 = alloca double, align 8
  %146 = alloca double, align 8
  %147 = alloca double, align 8
  %148 = alloca double, align 8
  %149 = alloca double, align 8
  %150 = alloca %class.Vector3_t, align 8
  %151 = alloca %"class.Eigen::CwiseNullaryOp.149", align 8
  %152 = alloca double, align 8
  %153 = alloca double, align 8
  %154 = alloca double, align 8
  %155 = alloca %"class.Eigen::Product.154", align 8
  %156 = alloca %class.Vector3_t, align 8
  %157 = alloca double, align 8
  %158 = alloca double, align 8
  %159 = alloca double, align 8
  %160 = alloca double, align 8
  %161 = alloca double, align 8
  %162 = alloca double, align 8
  %163 = alloca double, align 8
  %164 = alloca double, align 8
  %165 = alloca double, align 8
  %166 = alloca double, align 8
  %167 = alloca double, align 8
  %168 = alloca double, align 8
  %169 = alloca %"class.Eigen::Product.154", align 8
  %170 = alloca %class.Vector3_t, align 8
  %171 = alloca double, align 8
  %172 = alloca double, align 8
  %173 = alloca double, align 8
  %174 = alloca double, align 8
  %175 = alloca double, align 8
  %176 = alloca double, align 8
  %177 = alloca %"class.Eigen::CwiseBinaryOp.161", align 8
  %178 = alloca %"class.Eigen::Product.167", align 8
  %179 = alloca %"class.Eigen::Transpose", align 8
  %180 = alloca %class.Vector3_t, align 8
  %.not = icmp eq i32 %1, 0
  br i1 %.not, label %181, label %182

181:                                              ; preds = %4
  tail call void @__assert_fail(ptr noundef nonnull @.str, ptr noundef nonnull @.str.4, i32 noundef 37, ptr noundef nonnull @__PRETTY_FUNCTION__._ZN17RigidBodyDynamics5jcalcERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEES6_) #10
  unreachable

182:                                              ; preds = %4
  %183 = getelementptr inbounds i8, ptr %0, i64 160
  %184 = zext i32 %1 to i64
  %185 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %186 = getelementptr inbounds i8, ptr %185, i64 8
  %187 = load i32, ptr %186, align 8, !tbaa !5
  %188 = icmp eq i32 %187, 3
  br i1 %188, label %189, label %283

189:                                              ; preds = %182
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %5) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %6) #11
  %190 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %191 = getelementptr inbounds i8, ptr %190, i64 16
  %192 = load i32, ptr %191, align 8, !tbaa !12
  %193 = zext i32 %192 to i64
  %194 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %193)
  %195 = load double, ptr %194, align 8, !tbaa !13
  call void @sincos(double noundef %195, ptr noundef nonnull %5, ptr noundef nonnull %6) #11
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %7) #11
  %196 = getelementptr inbounds i8, ptr %0, i64 280
  %197 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %196, i64 noundef %184) #11
  %198 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %197, i64 noundef 0, i64 noundef 0)
  %199 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %196, i64 noundef %184) #11
  %200 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %199, i64 noundef 0, i64 noundef 1)
  %201 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %196, i64 noundef %184) #11
  %202 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %201, i64 noundef 0, i64 noundef 2)
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %8) #11
  %203 = load double, ptr %6, align 8, !tbaa !13
  %204 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %196, i64 noundef %184) #11
  %205 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %204, i64 noundef 1, i64 noundef 0)
  %206 = load double, ptr %205, align 8, !tbaa !13
  %207 = load double, ptr %5, align 8, !tbaa !13
  %208 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %196, i64 noundef %184) #11
  %209 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %208, i64 noundef 2, i64 noundef 0)
  %210 = load double, ptr %209, align 8, !tbaa !13
  %211 = fmul double %207, %210
  %212 = call double @llvm.fmuladd.f64(double %203, double %206, double %211)
  store double %212, ptr %8, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %9) #11
  %213 = load double, ptr %6, align 8, !tbaa !13
  %214 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %196, i64 noundef %184) #11
  %215 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %214, i64 noundef 1, i64 noundef 1)
  %216 = load double, ptr %215, align 8, !tbaa !13
  %217 = load double, ptr %5, align 8, !tbaa !13
  %218 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %196, i64 noundef %184) #11
  %219 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %218, i64 noundef 2, i64 noundef 1)
  %220 = load double, ptr %219, align 8, !tbaa !13
  %221 = fmul double %217, %220
  %222 = call double @llvm.fmuladd.f64(double %213, double %216, double %221)
  store double %222, ptr %9, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %10) #11
  %223 = load double, ptr %6, align 8, !tbaa !13
  %224 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %196, i64 noundef %184) #11
  %225 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %224, i64 noundef 1, i64 noundef 2)
  %226 = load double, ptr %225, align 8, !tbaa !13
  %227 = load double, ptr %5, align 8, !tbaa !13
  %228 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %196, i64 noundef %184) #11
  %229 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %228, i64 noundef 2, i64 noundef 2)
  %230 = load double, ptr %229, align 8, !tbaa !13
  %231 = fmul double %227, %230
  %232 = call double @llvm.fmuladd.f64(double %223, double %226, double %231)
  store double %232, ptr %10, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %11) #11
  %233 = load double, ptr %5, align 8, !tbaa !13
  %234 = fneg double %233
  %235 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %196, i64 noundef %184) #11
  %236 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %235, i64 noundef 1, i64 noundef 0)
  %237 = load double, ptr %236, align 8, !tbaa !13
  %238 = load double, ptr %6, align 8, !tbaa !13
  %239 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %196, i64 noundef %184) #11
  %240 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %239, i64 noundef 2, i64 noundef 0)
  %241 = load double, ptr %240, align 8, !tbaa !13
  %242 = fmul double %238, %241
  %243 = call double @llvm.fmuladd.f64(double %234, double %237, double %242)
  store double %243, ptr %11, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %12) #11
  %244 = load double, ptr %5, align 8, !tbaa !13
  %245 = fneg double %244
  %246 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %196, i64 noundef %184) #11
  %247 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %246, i64 noundef 1, i64 noundef 1)
  %248 = load double, ptr %247, align 8, !tbaa !13
  %249 = load double, ptr %6, align 8, !tbaa !13
  %250 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %196, i64 noundef %184) #11
  %251 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %250, i64 noundef 2, i64 noundef 1)
  %252 = load double, ptr %251, align 8, !tbaa !13
  %253 = fmul double %249, %252
  %254 = call double @llvm.fmuladd.f64(double %245, double %248, double %253)
  store double %254, ptr %12, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %13) #11
  %255 = load double, ptr %5, align 8, !tbaa !13
  %256 = fneg double %255
  %257 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %196, i64 noundef %184) #11
  %258 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %257, i64 noundef 1, i64 noundef 2)
  %259 = load double, ptr %258, align 8, !tbaa !13
  %260 = load double, ptr %6, align 8, !tbaa !13
  %261 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %196, i64 noundef %184) #11
  %262 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %261, i64 noundef 2, i64 noundef 2)
  %263 = load double, ptr %262, align 8, !tbaa !13
  %264 = fmul double %260, %263
  %265 = call double @llvm.fmuladd.f64(double %256, double %259, double %264)
  store double %265, ptr %13, align 8, !tbaa !13
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %7, ptr noundef nonnull align 8 dereferenceable(8) %198, ptr noundef nonnull align 8 dereferenceable(8) %200, ptr noundef nonnull align 8 dereferenceable(8) %202, ptr noundef nonnull align 8 dereferenceable(8) %8, ptr noundef nonnull align 8 dereferenceable(8) %9, ptr noundef nonnull align 8 dereferenceable(8) %10, ptr noundef nonnull align 8 dereferenceable(8) %11, ptr noundef nonnull align 8 dereferenceable(8) %12, ptr noundef nonnull align 8 dereferenceable(8) %13)
  %266 = getelementptr inbounds i8, ptr %0, i64 720
  %267 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %266, i64 noundef %184) #11
  %268 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(72) %267, ptr noundef nonnull align 8 dereferenceable(72) %7) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %13) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %12) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %11) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %10) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %9) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %8) #11
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %7) #11
  %269 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %196, i64 noundef %184) #11
  %270 = getelementptr inbounds i8, ptr %269, i64 72
  %271 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %266, i64 noundef %184) #11
  %272 = getelementptr inbounds i8, ptr %271, i64 72
  %273 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSERKS_(ptr noundef nonnull align 8 dereferenceable(24) %272, ptr noundef nonnull align 8 dereferenceable(24) %270)
  %274 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %275 = getelementptr inbounds i8, ptr %274, i64 16
  %276 = load i32, ptr %275, align 8, !tbaa !12
  %277 = zext i32 %276 to i64
  %278 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %277)
  %279 = load double, ptr %278, align 8, !tbaa !13
  %280 = getelementptr inbounds i8, ptr %0, i64 208
  %281 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %280, i64 noundef %184) #11
  %282 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %281, i64 noundef 0)
  store double %279, ptr %282, align 8, !tbaa !13
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %6) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %5) #11
  br label %1164

283:                                              ; preds = %182
  %284 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %285 = getelementptr inbounds i8, ptr %284, i64 8
  %286 = load i32, ptr %285, align 8, !tbaa !5
  %287 = icmp eq i32 %286, 4
  br i1 %287, label %288, label %382

288:                                              ; preds = %283
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %14) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %15) #11
  %289 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %290 = getelementptr inbounds i8, ptr %289, i64 16
  %291 = load i32, ptr %290, align 8, !tbaa !12
  %292 = zext i32 %291 to i64
  %293 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %292)
  %294 = load double, ptr %293, align 8, !tbaa !13
  call void @sincos(double noundef %294, ptr noundef nonnull %14, ptr noundef nonnull %15) #11
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %16) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %17) #11
  %295 = load double, ptr %15, align 8, !tbaa !13
  %296 = getelementptr inbounds i8, ptr %0, i64 280
  %297 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %296, i64 noundef %184) #11
  %298 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %297, i64 noundef 0, i64 noundef 0)
  %299 = load double, ptr %298, align 8, !tbaa !13
  %300 = load double, ptr %14, align 8, !tbaa !13
  %301 = fneg double %300
  %302 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %296, i64 noundef %184) #11
  %303 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %302, i64 noundef 2, i64 noundef 0)
  %304 = load double, ptr %303, align 8, !tbaa !13
  %305 = fmul double %304, %301
  %306 = call double @llvm.fmuladd.f64(double %295, double %299, double %305)
  store double %306, ptr %17, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %18) #11
  %307 = load double, ptr %15, align 8, !tbaa !13
  %308 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %296, i64 noundef %184) #11
  %309 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %308, i64 noundef 0, i64 noundef 1)
  %310 = load double, ptr %309, align 8, !tbaa !13
  %311 = load double, ptr %14, align 8, !tbaa !13
  %312 = fneg double %311
  %313 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %296, i64 noundef %184) #11
  %314 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %313, i64 noundef 2, i64 noundef 1)
  %315 = load double, ptr %314, align 8, !tbaa !13
  %316 = fmul double %315, %312
  %317 = call double @llvm.fmuladd.f64(double %307, double %310, double %316)
  store double %317, ptr %18, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %19) #11
  %318 = load double, ptr %15, align 8, !tbaa !13
  %319 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %296, i64 noundef %184) #11
  %320 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %319, i64 noundef 0, i64 noundef 2)
  %321 = load double, ptr %320, align 8, !tbaa !13
  %322 = load double, ptr %14, align 8, !tbaa !13
  %323 = fneg double %322
  %324 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %296, i64 noundef %184) #11
  %325 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %324, i64 noundef 2, i64 noundef 2)
  %326 = load double, ptr %325, align 8, !tbaa !13
  %327 = fmul double %326, %323
  %328 = call double @llvm.fmuladd.f64(double %318, double %321, double %327)
  store double %328, ptr %19, align 8, !tbaa !13
  %329 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %296, i64 noundef %184) #11
  %330 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %329, i64 noundef 1, i64 noundef 0)
  %331 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %296, i64 noundef %184) #11
  %332 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %331, i64 noundef 1, i64 noundef 1)
  %333 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %296, i64 noundef %184) #11
  %334 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %333, i64 noundef 1, i64 noundef 2)
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %20) #11
  %335 = load double, ptr %14, align 8, !tbaa !13
  %336 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %296, i64 noundef %184) #11
  %337 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %336, i64 noundef 0, i64 noundef 0)
  %338 = load double, ptr %337, align 8, !tbaa !13
  %339 = load double, ptr %15, align 8, !tbaa !13
  %340 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %296, i64 noundef %184) #11
  %341 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %340, i64 noundef 2, i64 noundef 0)
  %342 = load double, ptr %341, align 8, !tbaa !13
  %343 = fmul double %339, %342
  %344 = call double @llvm.fmuladd.f64(double %335, double %338, double %343)
  store double %344, ptr %20, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %21) #11
  %345 = load double, ptr %14, align 8, !tbaa !13
  %346 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %296, i64 noundef %184) #11
  %347 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %346, i64 noundef 0, i64 noundef 1)
  %348 = load double, ptr %347, align 8, !tbaa !13
  %349 = load double, ptr %15, align 8, !tbaa !13
  %350 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %296, i64 noundef %184) #11
  %351 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %350, i64 noundef 2, i64 noundef 1)
  %352 = load double, ptr %351, align 8, !tbaa !13
  %353 = fmul double %349, %352
  %354 = call double @llvm.fmuladd.f64(double %345, double %348, double %353)
  store double %354, ptr %21, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %22) #11
  %355 = load double, ptr %14, align 8, !tbaa !13
  %356 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %296, i64 noundef %184) #11
  %357 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %356, i64 noundef 0, i64 noundef 2)
  %358 = load double, ptr %357, align 8, !tbaa !13
  %359 = load double, ptr %15, align 8, !tbaa !13
  %360 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %296, i64 noundef %184) #11
  %361 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %360, i64 noundef 2, i64 noundef 2)
  %362 = load double, ptr %361, align 8, !tbaa !13
  %363 = fmul double %359, %362
  %364 = call double @llvm.fmuladd.f64(double %355, double %358, double %363)
  store double %364, ptr %22, align 8, !tbaa !13
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %16, ptr noundef nonnull align 8 dereferenceable(8) %17, ptr noundef nonnull align 8 dereferenceable(8) %18, ptr noundef nonnull align 8 dereferenceable(8) %19, ptr noundef nonnull align 8 dereferenceable(8) %330, ptr noundef nonnull align 8 dereferenceable(8) %332, ptr noundef nonnull align 8 dereferenceable(8) %334, ptr noundef nonnull align 8 dereferenceable(8) %20, ptr noundef nonnull align 8 dereferenceable(8) %21, ptr noundef nonnull align 8 dereferenceable(8) %22)
  %365 = getelementptr inbounds i8, ptr %0, i64 720
  %366 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %365, i64 noundef %184) #11
  %367 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(72) %366, ptr noundef nonnull align 8 dereferenceable(72) %16) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %22) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %21) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %20) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %19) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %18) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %17) #11
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %16) #11
  %368 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %296, i64 noundef %184) #11
  %369 = getelementptr inbounds i8, ptr %368, i64 72
  %370 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %365, i64 noundef %184) #11
  %371 = getelementptr inbounds i8, ptr %370, i64 72
  %372 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSERKS_(ptr noundef nonnull align 8 dereferenceable(24) %371, ptr noundef nonnull align 8 dereferenceable(24) %369)
  %373 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %374 = getelementptr inbounds i8, ptr %373, i64 16
  %375 = load i32, ptr %374, align 8, !tbaa !12
  %376 = zext i32 %375 to i64
  %377 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %376)
  %378 = load double, ptr %377, align 8, !tbaa !13
  %379 = getelementptr inbounds i8, ptr %0, i64 208
  %380 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %379, i64 noundef %184) #11
  %381 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %380, i64 noundef 1)
  store double %378, ptr %381, align 8, !tbaa !13
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %15) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %14) #11
  br label %1164

382:                                              ; preds = %283
  %383 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %384 = getelementptr inbounds i8, ptr %383, i64 8
  %385 = load i32, ptr %384, align 8, !tbaa !5
  %386 = icmp eq i32 %385, 5
  br i1 %386, label %387, label %481

387:                                              ; preds = %382
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %23) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %24) #11
  %388 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %389 = getelementptr inbounds i8, ptr %388, i64 16
  %390 = load i32, ptr %389, align 8, !tbaa !12
  %391 = zext i32 %390 to i64
  %392 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %391)
  %393 = load double, ptr %392, align 8, !tbaa !13
  call void @sincos(double noundef %393, ptr noundef nonnull %23, ptr noundef nonnull %24) #11
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %25) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %26) #11
  %394 = load double, ptr %24, align 8, !tbaa !13
  %395 = getelementptr inbounds i8, ptr %0, i64 280
  %396 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %395, i64 noundef %184) #11
  %397 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %396, i64 noundef 0, i64 noundef 0)
  %398 = load double, ptr %397, align 8, !tbaa !13
  %399 = load double, ptr %23, align 8, !tbaa !13
  %400 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %395, i64 noundef %184) #11
  %401 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %400, i64 noundef 1, i64 noundef 0)
  %402 = load double, ptr %401, align 8, !tbaa !13
  %403 = fmul double %399, %402
  %404 = call double @llvm.fmuladd.f64(double %394, double %398, double %403)
  store double %404, ptr %26, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %27) #11
  %405 = load double, ptr %24, align 8, !tbaa !13
  %406 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %395, i64 noundef %184) #11
  %407 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %406, i64 noundef 0, i64 noundef 1)
  %408 = load double, ptr %407, align 8, !tbaa !13
  %409 = load double, ptr %23, align 8, !tbaa !13
  %410 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %395, i64 noundef %184) #11
  %411 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %410, i64 noundef 1, i64 noundef 1)
  %412 = load double, ptr %411, align 8, !tbaa !13
  %413 = fmul double %409, %412
  %414 = call double @llvm.fmuladd.f64(double %405, double %408, double %413)
  store double %414, ptr %27, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %28) #11
  %415 = load double, ptr %24, align 8, !tbaa !13
  %416 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %395, i64 noundef %184) #11
  %417 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %416, i64 noundef 0, i64 noundef 2)
  %418 = load double, ptr %417, align 8, !tbaa !13
  %419 = load double, ptr %23, align 8, !tbaa !13
  %420 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %395, i64 noundef %184) #11
  %421 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %420, i64 noundef 1, i64 noundef 2)
  %422 = load double, ptr %421, align 8, !tbaa !13
  %423 = fmul double %419, %422
  %424 = call double @llvm.fmuladd.f64(double %415, double %418, double %423)
  store double %424, ptr %28, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %29) #11
  %425 = load double, ptr %23, align 8, !tbaa !13
  %426 = fneg double %425
  %427 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %395, i64 noundef %184) #11
  %428 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %427, i64 noundef 0, i64 noundef 0)
  %429 = load double, ptr %428, align 8, !tbaa !13
  %430 = load double, ptr %24, align 8, !tbaa !13
  %431 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %395, i64 noundef %184) #11
  %432 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %431, i64 noundef 1, i64 noundef 0)
  %433 = load double, ptr %432, align 8, !tbaa !13
  %434 = fmul double %430, %433
  %435 = call double @llvm.fmuladd.f64(double %426, double %429, double %434)
  store double %435, ptr %29, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %30) #11
  %436 = load double, ptr %23, align 8, !tbaa !13
  %437 = fneg double %436
  %438 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %395, i64 noundef %184) #11
  %439 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %438, i64 noundef 0, i64 noundef 1)
  %440 = load double, ptr %439, align 8, !tbaa !13
  %441 = load double, ptr %24, align 8, !tbaa !13
  %442 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %395, i64 noundef %184) #11
  %443 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %442, i64 noundef 1, i64 noundef 1)
  %444 = load double, ptr %443, align 8, !tbaa !13
  %445 = fmul double %441, %444
  %446 = call double @llvm.fmuladd.f64(double %437, double %440, double %445)
  store double %446, ptr %30, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %31) #11
  %447 = load double, ptr %23, align 8, !tbaa !13
  %448 = fneg double %447
  %449 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %395, i64 noundef %184) #11
  %450 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %449, i64 noundef 0, i64 noundef 2)
  %451 = load double, ptr %450, align 8, !tbaa !13
  %452 = load double, ptr %24, align 8, !tbaa !13
  %453 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %395, i64 noundef %184) #11
  %454 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %453, i64 noundef 1, i64 noundef 2)
  %455 = load double, ptr %454, align 8, !tbaa !13
  %456 = fmul double %452, %455
  %457 = call double @llvm.fmuladd.f64(double %448, double %451, double %456)
  store double %457, ptr %31, align 8, !tbaa !13
  %458 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %395, i64 noundef %184) #11
  %459 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %458, i64 noundef 2, i64 noundef 0)
  %460 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %395, i64 noundef %184) #11
  %461 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %460, i64 noundef 2, i64 noundef 1)
  %462 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %395, i64 noundef %184) #11
  %463 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %462, i64 noundef 2, i64 noundef 2)
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %25, ptr noundef nonnull align 8 dereferenceable(8) %26, ptr noundef nonnull align 8 dereferenceable(8) %27, ptr noundef nonnull align 8 dereferenceable(8) %28, ptr noundef nonnull align 8 dereferenceable(8) %29, ptr noundef nonnull align 8 dereferenceable(8) %30, ptr noundef nonnull align 8 dereferenceable(8) %31, ptr noundef nonnull align 8 dereferenceable(8) %459, ptr noundef nonnull align 8 dereferenceable(8) %461, ptr noundef nonnull align 8 dereferenceable(8) %463)
  %464 = getelementptr inbounds i8, ptr %0, i64 720
  %465 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %464, i64 noundef %184) #11
  %466 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(72) %465, ptr noundef nonnull align 8 dereferenceable(72) %25) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %31) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %30) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %29) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %28) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %27) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %26) #11
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %25) #11
  %467 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %395, i64 noundef %184) #11
  %468 = getelementptr inbounds i8, ptr %467, i64 72
  %469 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %464, i64 noundef %184) #11
  %470 = getelementptr inbounds i8, ptr %469, i64 72
  %471 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSERKS_(ptr noundef nonnull align 8 dereferenceable(24) %470, ptr noundef nonnull align 8 dereferenceable(24) %468)
  %472 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %473 = getelementptr inbounds i8, ptr %472, i64 16
  %474 = load i32, ptr %473, align 8, !tbaa !12
  %475 = zext i32 %474 to i64
  %476 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %475)
  %477 = load double, ptr %476, align 8, !tbaa !13
  %478 = getelementptr inbounds i8, ptr %0, i64 208
  %479 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %478, i64 noundef %184) #11
  %480 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %479, i64 noundef 2)
  store double %477, ptr %480, align 8, !tbaa !13
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %24) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %23) #11
  br label %1164

481:                                              ; preds = %382
  %482 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %483 = getelementptr inbounds i8, ptr %482, i64 8
  %484 = load i32, ptr %483, align 8, !tbaa !5
  %485 = icmp eq i32 %484, 14
  br i1 %485, label %486, label %517

486:                                              ; preds = %481
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %32) #11
  call void @_ZN17RigidBodyDynamics8jcalc_XJERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %32, ptr noundef nonnull align 8 dereferenceable(872) %0, i32 noundef %1, ptr noundef nonnull align 8 dereferenceable(16) %2)
  call void @_ZN17RigidBodyDynamics16jcalc_X_lambda_SERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr noundef nonnull align 8 dereferenceable(872) %0, i32 noundef %1, ptr noundef nonnull align 8 dereferenceable(16) %2)
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %33) #11
  %487 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %488 = getelementptr inbounds i8, ptr %487, i64 16
  %489 = load i32, ptr %488, align 8, !tbaa !12
  %490 = zext i32 %489 to i64
  %491 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %490)
  %492 = load double, ptr %491, align 8, !tbaa !13
  store double %492, ptr %33, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 32, ptr nonnull %34) #11
  %493 = getelementptr inbounds i8, ptr %0, i64 184
  %494 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %493, i64 noundef %184) #11
  call void @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEEmlIdEEKNS_13CwiseBinaryOpINS_8internal17scalar_product_opIdNS6_18promote_scalar_argIdT_Xsr5Eigen8internal14has_ReturnTypeINS_20ScalarBinaryOpTraitsIdS9_NS7_IdS9_EEEEEE5valueEE4typeEEEKS2_KNS6_19plain_constant_typeIS2_SE_E4typeEEERKS9_(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseBinaryOp") align 8 %34, ptr noundef nonnull align 1 dereferenceable(1) %494, ptr noundef nonnull align 8 dereferenceable(8) %33)
  %495 = getelementptr inbounds i8, ptr %0, i64 208
  %496 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %495, i64 noundef %184) #11
  %497 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen13CwiseBinaryOpINS1_8internal17scalar_product_opIddEEKNS1_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEKNS1_14CwiseNullaryOpINS3_18scalar_constant_opIdEES8_EEEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48) %496, ptr noundef nonnull align 1 dereferenceable(1) %34)
  call void @llvm.lifetime.end.p0(i64 32, ptr nonnull %34) #11
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %35) #11
  call void @llvm.lifetime.start.p0(i64 56, ptr nonnull %36) #11
  %498 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %493, i64 noundef %184) #11
  call void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEE5blockIiiEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::Block") align 8 %36, ptr noundef nonnull align 1 dereferenceable(1) %498, i64 noundef 0, i64 noundef 0, i32 noundef 3, i32 noundef 1)
  call void @_ZN9Vector3_tC2IN5Eigen5BlockINS1_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELin1ELin1ELb0EEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %35, ptr noundef nonnull align 1 dereferenceable(1) %36)
  call void @llvm.lifetime.end.p0(i64 56, ptr nonnull %36) #11
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %37) #11
  call void @llvm.lifetime.start.p0(i64 64, ptr nonnull %38) #11
  call void @llvm.lifetime.start.p0(i64 56, ptr nonnull %39) #11
  %499 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %500 = load ptr, ptr %499, align 8, !tbaa !15
  call void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEE5blockIiiEENS3_13FixedBlockXprIXsr8internal15get_fixed_valueIT_EE5valueEXsr8internal15get_fixed_valueIT0_EE5valueEE4TypeEllS6_S7_(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::Block") align 8 %39, ptr noundef nonnull align 1 dereferenceable(1) %500, i64 noundef 3, i64 noundef 0, i32 noundef 3, i32 noundef 1)
  call void @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEmlINS_5BlockINS1_IdLi6ELi1ELi0ELi6ELi1EEELin1ELin1ELb0EEEEEKNS_7ProductIS2_T_Li0EEERKNS0_IS9_EE(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::Product") align 8 %38, ptr noundef nonnull align 1 dereferenceable(1) %32, ptr noundef nonnull align 1 dereferenceable(1) %39)
  call void @_ZN9Vector3_tC2IN5Eigen7ProductINS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEENS1_5BlockINS3_IdLi6ELi1ELi0ELi6ELi1EEELin1ELin1ELb0EEELi0EEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %37, ptr noundef nonnull align 1 dereferenceable(1) %38)
  call void @llvm.lifetime.end.p0(i64 56, ptr nonnull %39) #11
  call void @llvm.lifetime.end.p0(i64 64, ptr nonnull %38) #11
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %40) #11
  call void @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE5crossIS2_EENS3_25cross_product_return_typeIT_E4typeERKNS0_IS6_EE(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::Matrix") align 8 %40, ptr noundef nonnull align 1 dereferenceable(1) %35, ptr noundef nonnull align 1 dereferenceable(1) %37)
  %501 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSIN5Eigen6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %37, ptr noundef nonnull align 1 dereferenceable(1) %40)
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %40) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %41) #11
  %502 = load double, ptr %33, align 8, !tbaa !13
  %503 = fneg double %502
  %504 = fmul double %502, %503
  store double %504, ptr %41, align 8, !tbaa !13
  %505 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEmLERKd(ptr noundef nonnull align 1 dereferenceable(1) %37, ptr noundef nonnull align 8 dereferenceable(8) %41)
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %41) #11
  call void @llvm.lifetime.start.p0(i64 48, ptr nonnull %42) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %43) #11
  store double 0.000000e+00, ptr %43, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %44) #11
  store double 0.000000e+00, ptr %44, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %45) #11
  store double 0.000000e+00, ptr %45, align 8, !tbaa !13
  %506 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %37, i64 noundef 0)
  %507 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %37, i64 noundef 1)
  %508 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %37, i64 noundef 2)
  call void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %42, ptr noundef nonnull align 8 dereferenceable(8) %43, ptr noundef nonnull align 8 dereferenceable(8) %44, ptr noundef nonnull align 8 dereferenceable(8) %45, ptr noundef nonnull align 8 dereferenceable(8) %506, ptr noundef nonnull align 8 dereferenceable(8) %507, ptr noundef nonnull align 8 dereferenceable(8) %508)
  %509 = getelementptr inbounds i8, ptr %0, i64 232
  %510 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %509, i64 noundef %184) #11
  %511 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSEOS_(ptr noundef nonnull align 16 dereferenceable(48) %510, ptr noundef nonnull align 16 dereferenceable(48) %42) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %45) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %44) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %43) #11
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %42) #11
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %46) #11
  %512 = getelementptr inbounds i8, ptr %0, i64 280
  %513 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %512, i64 noundef %184) #11
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %46, ptr noundef nonnull align 8 dereferenceable(96) %32, ptr noundef nonnull align 8 dereferenceable(96) %513)
  %514 = getelementptr inbounds i8, ptr %0, i64 720
  %515 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %514, i64 noundef %184) #11
  %516 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %515, ptr noundef nonnull align 8 dereferenceable(96) %46) #11
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %46) #11
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %37) #11
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %35) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %33) #11
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %32) #11
  br label %1164

517:                                              ; preds = %481
  %518 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %519 = getelementptr inbounds i8, ptr %518, i64 12
  %520 = load i32, ptr %519, align 4, !tbaa !16
  %521 = icmp eq i32 %520, 1
  br i1 %521, label %522, label %542

522:                                              ; preds = %517
  %523 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %524 = getelementptr inbounds i8, ptr %523, i64 8
  %525 = load i32, ptr %524, align 8, !tbaa !5
  %.not627 = icmp eq i32 %525, 21
  br i1 %.not627, label %542, label %526

526:                                              ; preds = %522
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %47) #11
  call void @_ZN17RigidBodyDynamics8jcalc_XJERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %47, ptr noundef nonnull align 8 dereferenceable(872) %0, i32 noundef %1, ptr noundef nonnull align 8 dereferenceable(16) %2)
  call void @llvm.lifetime.start.p0(i64 32, ptr nonnull %48) #11
  %527 = getelementptr inbounds i8, ptr %0, i64 184
  %528 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %527, i64 noundef %184) #11
  %529 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %530 = getelementptr inbounds i8, ptr %529, i64 16
  %531 = load i32, ptr %530, align 8, !tbaa !12
  %532 = zext i32 %531 to i64
  %533 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %532)
  call void @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEEmlIdEEKNS_13CwiseBinaryOpINS_8internal17scalar_product_opIdNS6_18promote_scalar_argIdT_Xsr5Eigen8internal14has_ReturnTypeINS_20ScalarBinaryOpTraitsIdS9_NS7_IdS9_EEEEEE5valueEE4typeEEEKS2_KNS6_19plain_constant_typeIS2_SE_E4typeEEERKS9_(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseBinaryOp") align 8 %48, ptr noundef nonnull align 1 dereferenceable(1) %528, ptr noundef nonnull align 8 dereferenceable(8) %533)
  %534 = getelementptr inbounds i8, ptr %0, i64 208
  %535 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %534, i64 noundef %184) #11
  %536 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen13CwiseBinaryOpINS1_8internal17scalar_product_opIddEEKNS1_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEKNS1_14CwiseNullaryOpINS3_18scalar_constant_opIdEES8_EEEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48) %535, ptr noundef nonnull align 1 dereferenceable(1) %48)
  call void @llvm.lifetime.end.p0(i64 32, ptr nonnull %48) #11
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %49) #11
  %537 = getelementptr inbounds i8, ptr %0, i64 280
  %538 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %537, i64 noundef %184) #11
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %49, ptr noundef nonnull align 8 dereferenceable(96) %47, ptr noundef nonnull align 8 dereferenceable(96) %538)
  %539 = getelementptr inbounds i8, ptr %0, i64 720
  %540 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %539, i64 noundef %184) #11
  %541 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %540, ptr noundef nonnull align 8 dereferenceable(96) %49) #11
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %49) #11
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %47) #11
  br label %1164

542:                                              ; preds = %522, %517
  %543 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %544 = getelementptr inbounds i8, ptr %543, i64 8
  %545 = load i32, ptr %544, align 8, !tbaa !5
  %546 = icmp eq i32 %545, 6
  br i1 %546, label %547, label %583

547:                                              ; preds = %542
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %50) #11
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %51) #11
  call void @llvm.lifetime.start.p0(i64 32, ptr nonnull %52) #11
  call void @_ZNK17RigidBodyDynamics5Model13GetQuaternionEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr dead_on_unwind nonnull writable sret(%"class.RigidBodyDynamics::Math::Quaternion") align 16 %52, ptr noundef nonnull align 8 dereferenceable(872) %0, i32 noundef %1, ptr noundef nonnull align 8 dereferenceable(16) %2)
  call void @_ZNK17RigidBodyDynamics4Math10Quaternion8toMatrixEv(ptr dead_on_unwind nonnull writable sret(%class.Matrix3_t) align 8 %51, ptr noundef nonnull align 16 dereferenceable(32) %52)
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %53) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %54) #11
  store double 0.000000e+00, ptr %54, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %55) #11
  store double 0.000000e+00, ptr %55, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %56) #11
  store double 0.000000e+00, ptr %56, align 8, !tbaa !13
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %53, ptr noundef nonnull align 8 dereferenceable(8) %54, ptr noundef nonnull align 8 dereferenceable(8) %55, ptr noundef nonnull align 8 dereferenceable(8) %56)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %50, ptr noundef nonnull align 8 dereferenceable(72) %51, ptr noundef nonnull align 8 dereferenceable(24) %53)
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %56) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %55) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %54) #11
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %53) #11
  call void @llvm.lifetime.end.p0(i64 32, ptr nonnull %52) #11
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %51) #11
  %548 = getelementptr inbounds i8, ptr %0, i64 328
  %549 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %548, i64 noundef %184) #11
  %550 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %549, i64 noundef 0, i64 noundef 0)
  store double 1.000000e+00, ptr %550, align 8, !tbaa !13
  %551 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %548, i64 noundef %184) #11
  %552 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %551, i64 noundef 1, i64 noundef 1)
  store double 1.000000e+00, ptr %552, align 8, !tbaa !13
  %553 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %548, i64 noundef %184) #11
  %554 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %553, i64 noundef 2, i64 noundef 2)
  store double 1.000000e+00, ptr %554, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %57) #11
  %555 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %556 = getelementptr inbounds i8, ptr %555, i64 16
  %557 = load i32, ptr %556, align 8, !tbaa !12
  %558 = zext i32 %557 to i64
  %559 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %558)
  %560 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %561 = getelementptr inbounds i8, ptr %560, i64 16
  %562 = load i32, ptr %561, align 8, !tbaa !12
  %563 = add i32 %562, 1
  %564 = zext i32 %563 to i64
  %565 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %564)
  %566 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %567 = getelementptr inbounds i8, ptr %566, i64 16
  %568 = load i32, ptr %567, align 8, !tbaa !12
  %569 = add i32 %568, 2
  %570 = zext i32 %569 to i64
  %571 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %570)
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %57, ptr noundef nonnull align 8 dereferenceable(8) %559, ptr noundef nonnull align 8 dereferenceable(8) %565, ptr noundef nonnull align 8 dereferenceable(8) %571)
  call void @llvm.lifetime.start.p0(i64 48, ptr nonnull %58) #11
  %572 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %57, i64 noundef 0)
  %573 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %57, i64 noundef 1)
  %574 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %57, i64 noundef 2)
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %59) #11
  store double 0.000000e+00, ptr %59, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %60) #11
  store double 0.000000e+00, ptr %60, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %61) #11
  store double 0.000000e+00, ptr %61, align 8, !tbaa !13
  call void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %58, ptr noundef nonnull align 8 dereferenceable(8) %572, ptr noundef nonnull align 8 dereferenceable(8) %573, ptr noundef nonnull align 8 dereferenceable(8) %574, ptr noundef nonnull align 8 dereferenceable(8) %59, ptr noundef nonnull align 8 dereferenceable(8) %60, ptr noundef nonnull align 8 dereferenceable(8) %61)
  %575 = getelementptr inbounds i8, ptr %0, i64 208
  %576 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %575, i64 noundef %184) #11
  %577 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSEOS_(ptr noundef nonnull align 16 dereferenceable(48) %576, ptr noundef nonnull align 16 dereferenceable(48) %58) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %61) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %60) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %59) #11
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %58) #11
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %62) #11
  %578 = getelementptr inbounds i8, ptr %0, i64 280
  %579 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %578, i64 noundef %184) #11
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %62, ptr noundef nonnull align 8 dereferenceable(96) %50, ptr noundef nonnull align 8 dereferenceable(96) %579)
  %580 = getelementptr inbounds i8, ptr %0, i64 720
  %581 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %580, i64 noundef %184) #11
  %582 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %581, ptr noundef nonnull align 8 dereferenceable(96) %62) #11
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %62) #11
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %57) #11
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %50) #11
  br label %1164

583:                                              ; preds = %542
  %584 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %585 = getelementptr inbounds i8, ptr %584, i64 8
  %586 = load i32, ptr %585, align 8, !tbaa !5
  %587 = icmp eq i32 %586, 7
  %588 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  br i1 %587, label %589, label %703

589:                                              ; preds = %583
  %590 = getelementptr inbounds i8, ptr %588, i64 16
  %591 = load i32, ptr %590, align 8, !tbaa !12
  %592 = zext i32 %591 to i64
  %593 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %592)
  %594 = load double, ptr %593, align 8, !tbaa !13
  %595 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %596 = getelementptr inbounds i8, ptr %595, i64 16
  %597 = load i32, ptr %596, align 8, !tbaa !12
  %598 = add i32 %597, 1
  %599 = zext i32 %598 to i64
  %600 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %599)
  %601 = load double, ptr %600, align 8, !tbaa !13
  %602 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %603 = getelementptr inbounds i8, ptr %602, i64 16
  %604 = load i32, ptr %603, align 8, !tbaa !12
  %605 = add i32 %604, 2
  %606 = zext i32 %605 to i64
  %607 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %606)
  %608 = load double, ptr %607, align 8, !tbaa !13
  %609 = tail call double @sin(double noundef %594) #11
  %610 = tail call double @cos(double noundef %594) #11
  %611 = tail call double @sin(double noundef %601) #11
  %612 = tail call double @cos(double noundef %601) #11
  %613 = tail call double @sin(double noundef %608) #11
  %614 = tail call double @cos(double noundef %608) #11
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %63) #11
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %64) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %65) #11
  %615 = fmul double %610, %612
  store double %615, ptr %65, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %66) #11
  %616 = fmul double %609, %612
  store double %616, ptr %66, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %67) #11
  %617 = fneg double %611
  store double %617, ptr %67, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %68) #11
  %618 = fmul double %610, %611
  %619 = fneg double %609
  %620 = fmul double %614, %619
  %621 = tail call double @llvm.fmuladd.f64(double %618, double %613, double %620)
  store double %621, ptr %68, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %69) #11
  %622 = fmul double %609, %611
  %623 = fmul double %610, %614
  %624 = tail call double @llvm.fmuladd.f64(double %622, double %613, double %623)
  store double %624, ptr %69, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %70) #11
  %625 = fmul double %612, %613
  store double %625, ptr %70, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %71) #11
  %626 = fmul double %609, %613
  %627 = tail call double @llvm.fmuladd.f64(double %618, double %614, double %626)
  store double %627, ptr %71, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %72) #11
  %628 = fneg double %610
  %629 = fmul double %613, %628
  %630 = tail call double @llvm.fmuladd.f64(double %622, double %614, double %629)
  store double %630, ptr %72, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %73) #11
  %631 = fmul double %612, %614
  store double %631, ptr %73, align 8, !tbaa !13
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %64, ptr noundef nonnull align 8 dereferenceable(8) %65, ptr noundef nonnull align 8 dereferenceable(8) %66, ptr noundef nonnull align 8 dereferenceable(8) %67, ptr noundef nonnull align 8 dereferenceable(8) %68, ptr noundef nonnull align 8 dereferenceable(8) %69, ptr noundef nonnull align 8 dereferenceable(8) %70, ptr noundef nonnull align 8 dereferenceable(8) %71, ptr noundef nonnull align 8 dereferenceable(8) %72, ptr noundef nonnull align 8 dereferenceable(8) %73)
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %74) #11
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %75) #11
  call void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE4ZeroEv(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseNullaryOp.149") align 8 %75)
  call void @_ZN9Vector3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %74, ptr noundef nonnull align 1 dereferenceable(1) %75)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %63, ptr noundef nonnull align 8 dereferenceable(72) %64, ptr noundef nonnull align 8 dereferenceable(24) %74)
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %75) #11
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %74) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %73) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %72) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %71) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %70) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %69) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %68) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %67) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %66) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %65) #11
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %64) #11
  %632 = getelementptr inbounds i8, ptr %0, i64 328
  %633 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %632, i64 noundef %184) #11
  %634 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %633, i64 noundef 0, i64 noundef 0)
  store double %617, ptr %634, align 8, !tbaa !13
  %635 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %632, i64 noundef %184) #11
  %636 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %635, i64 noundef 0, i64 noundef 2)
  store double 1.000000e+00, ptr %636, align 8, !tbaa !13
  %637 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %632, i64 noundef %184) #11
  %638 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %637, i64 noundef 1, i64 noundef 0)
  store double %625, ptr %638, align 8, !tbaa !13
  %639 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %632, i64 noundef %184) #11
  %640 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %639, i64 noundef 1, i64 noundef 1)
  store double %614, ptr %640, align 8, !tbaa !13
  %641 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %632, i64 noundef %184) #11
  %642 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %641, i64 noundef 2, i64 noundef 0)
  store double %631, ptr %642, align 8, !tbaa !13
  %643 = fneg double %613
  %644 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %632, i64 noundef %184) #11
  %645 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %644, i64 noundef 2, i64 noundef 1)
  store double %643, ptr %645, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %76) #11
  %646 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %647 = getelementptr inbounds i8, ptr %646, i64 16
  %648 = load i32, ptr %647, align 8, !tbaa !12
  %649 = zext i32 %648 to i64
  %650 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %649)
  %651 = load double, ptr %650, align 8, !tbaa !13
  store double %651, ptr %76, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %77) #11
  %652 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %653 = getelementptr inbounds i8, ptr %652, i64 16
  %654 = load i32, ptr %653, align 8, !tbaa !12
  %655 = add i32 %654, 1
  %656 = zext i32 %655 to i64
  %657 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %656)
  %658 = load double, ptr %657, align 8, !tbaa !13
  store double %658, ptr %77, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %78) #11
  %659 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %660 = getelementptr inbounds i8, ptr %659, i64 16
  %661 = load i32, ptr %660, align 8, !tbaa !12
  %662 = add i32 %661, 2
  %663 = zext i32 %662 to i64
  %664 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %663)
  %665 = load double, ptr %664, align 8, !tbaa !13
  store double %665, ptr %78, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %79) #11
  %666 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %632, i64 noundef %184) #11
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %80) #11
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %80, ptr noundef nonnull align 8 dereferenceable(8) %76, ptr noundef nonnull align 8 dereferenceable(8) %77, ptr noundef nonnull align 8 dereferenceable(8) %78)
  %667 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEEEmlINS1_IdLi3ELi1ELi0ELi3ELi1EEEEEKNS_7ProductIS2_T_Li0EEERKNS0_IS7_EE(ptr noundef nonnull align 1 dereferenceable(1) %666, ptr noundef nonnull align 1 dereferenceable(1) %80)
  %668 = extractvalue { ptr, ptr } %667, 0
  store ptr %668, ptr %79, align 8
  %669 = getelementptr inbounds i8, ptr %79, i64 8
  %670 = extractvalue { ptr, ptr } %667, 1
  store ptr %670, ptr %669, align 8
  %671 = getelementptr inbounds i8, ptr %0, i64 208
  %672 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %671, i64 noundef %184) #11
  %673 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen7ProductINS1_6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS3_IdLi3ELi1ELi0ELi3ELi1EEELi0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48) %672, ptr noundef nonnull align 1 dereferenceable(1) %79)
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %80) #11
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %79) #11
  %674 = getelementptr inbounds i8, ptr %0, i64 232
  %675 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %674, i64 noundef %184) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %81) #11
  %676 = fneg double %612
  %677 = load double, ptr %76, align 8, !tbaa !13
  %678 = fmul double %677, %676
  %679 = load double, ptr %77, align 8, !tbaa !13
  %680 = fmul double %678, %679
  store double %680, ptr %81, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %82) #11
  %681 = fmul double %613, %617
  %682 = fmul double %681, %677
  %683 = fmul double %631, %677
  %684 = load double, ptr %78, align 8, !tbaa !13
  %685 = fmul double %683, %684
  %686 = call double @llvm.fmuladd.f64(double %682, double %679, double %685)
  %687 = fmul double %679, %643
  %688 = call double @llvm.fmuladd.f64(double %687, double %684, double %686)
  store double %688, ptr %82, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %83) #11
  %689 = fmul double %614, %617
  %690 = fmul double %689, %677
  %691 = fneg double %625
  %692 = fmul double %677, %691
  %693 = fmul double %692, %684
  %694 = call double @llvm.fmuladd.f64(double %690, double %679, double %693)
  %695 = fneg double %614
  %696 = fmul double %679, %695
  %697 = call double @llvm.fmuladd.f64(double %696, double %684, double %694)
  store double %697, ptr %83, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %84) #11
  store double 0.000000e+00, ptr %84, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %85) #11
  store double 0.000000e+00, ptr %85, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %86) #11
  store double 0.000000e+00, ptr %86, align 8, !tbaa !13
  call void @_ZN15SpatialVector_t3setERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %675, ptr noundef nonnull align 8 dereferenceable(8) %81, ptr noundef nonnull align 8 dereferenceable(8) %82, ptr noundef nonnull align 8 dereferenceable(8) %83, ptr noundef nonnull align 8 dereferenceable(8) %84, ptr noundef nonnull align 8 dereferenceable(8) %85, ptr noundef nonnull align 8 dereferenceable(8) %86)
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %86) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %85) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %84) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %83) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %82) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %81) #11
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %87) #11
  %698 = getelementptr inbounds i8, ptr %0, i64 280
  %699 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %698, i64 noundef %184) #11
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %87, ptr noundef nonnull align 8 dereferenceable(96) %63, ptr noundef nonnull align 8 dereferenceable(96) %699)
  %700 = getelementptr inbounds i8, ptr %0, i64 720
  %701 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %700, i64 noundef %184) #11
  %702 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %701, ptr noundef nonnull align 8 dereferenceable(96) %87) #11
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %87) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %78) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %77) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %76) #11
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %63) #11
  br label %1164

703:                                              ; preds = %583
  %704 = getelementptr inbounds i8, ptr %588, i64 8
  %705 = load i32, ptr %704, align 8, !tbaa !5
  %706 = icmp eq i32 %705, 8
  %707 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  br i1 %706, label %708, label %823

708:                                              ; preds = %703
  %709 = getelementptr inbounds i8, ptr %707, i64 16
  %710 = load i32, ptr %709, align 8, !tbaa !12
  %711 = zext i32 %710 to i64
  %712 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %711)
  %713 = load double, ptr %712, align 8, !tbaa !13
  %714 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %715 = getelementptr inbounds i8, ptr %714, i64 16
  %716 = load i32, ptr %715, align 8, !tbaa !12
  %717 = add i32 %716, 1
  %718 = zext i32 %717 to i64
  %719 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %718)
  %720 = load double, ptr %719, align 8, !tbaa !13
  %721 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %722 = getelementptr inbounds i8, ptr %721, i64 16
  %723 = load i32, ptr %722, align 8, !tbaa !12
  %724 = add i32 %723, 2
  %725 = zext i32 %724 to i64
  %726 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %725)
  %727 = load double, ptr %726, align 8, !tbaa !13
  %728 = tail call double @sin(double noundef %713) #11
  %729 = tail call double @cos(double noundef %713) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %88) #11
  %730 = tail call double @sin(double noundef %720) #11
  store double %730, ptr %88, align 8, !tbaa !13
  %731 = tail call double @cos(double noundef %720) #11
  %732 = tail call double @sin(double noundef %727) #11
  %733 = tail call double @cos(double noundef %727) #11
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %89) #11
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %90) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %91) #11
  %734 = fmul double %731, %733
  store double %734, ptr %91, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %92) #11
  %735 = fmul double %733, %730
  %736 = fmul double %728, %735
  %737 = tail call double @llvm.fmuladd.f64(double %732, double %729, double %736)
  store double %737, ptr %92, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %93) #11
  %738 = fneg double %733
  %739 = fmul double %730, %738
  %740 = fmul double %729, %739
  %741 = tail call double @llvm.fmuladd.f64(double %732, double %728, double %740)
  store double %741, ptr %93, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %94) #11
  %742 = fneg double %732
  %743 = fmul double %731, %742
  store double %743, ptr %94, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %95) #11
  %744 = fmul double %730, %742
  %745 = fmul double %728, %744
  %746 = tail call double @llvm.fmuladd.f64(double %733, double %729, double %745)
  store double %746, ptr %95, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %96) #11
  %747 = fmul double %732, %730
  %748 = fmul double %729, %747
  %749 = tail call double @llvm.fmuladd.f64(double %733, double %728, double %748)
  store double %749, ptr %96, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %97) #11
  %750 = fneg double %731
  %751 = fmul double %728, %750
  store double %751, ptr %97, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %98) #11
  %752 = fmul double %729, %731
  store double %752, ptr %98, align 8, !tbaa !13
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %90, ptr noundef nonnull align 8 dereferenceable(8) %91, ptr noundef nonnull align 8 dereferenceable(8) %92, ptr noundef nonnull align 8 dereferenceable(8) %93, ptr noundef nonnull align 8 dereferenceable(8) %94, ptr noundef nonnull align 8 dereferenceable(8) %95, ptr noundef nonnull align 8 dereferenceable(8) %96, ptr noundef nonnull align 8 dereferenceable(8) %88, ptr noundef nonnull align 8 dereferenceable(8) %97, ptr noundef nonnull align 8 dereferenceable(8) %98)
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %99) #11
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %100) #11
  call void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE4ZeroEv(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseNullaryOp.149") align 8 %100)
  call void @_ZN9Vector3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %99, ptr noundef nonnull align 1 dereferenceable(1) %100)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %89, ptr noundef nonnull align 8 dereferenceable(72) %90, ptr noundef nonnull align 8 dereferenceable(24) %99)
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %100) #11
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %99) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %98) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %97) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %96) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %95) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %94) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %93) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %92) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %91) #11
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %90) #11
  %753 = getelementptr inbounds i8, ptr %0, i64 328
  %754 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %753, i64 noundef %184) #11
  %755 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %754, i64 noundef 0, i64 noundef 0)
  store double %734, ptr %755, align 8, !tbaa !13
  %756 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %753, i64 noundef %184) #11
  %757 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %756, i64 noundef 0, i64 noundef 1)
  store double %732, ptr %757, align 8, !tbaa !13
  %758 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %753, i64 noundef %184) #11
  %759 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %758, i64 noundef 1, i64 noundef 0)
  store double %743, ptr %759, align 8, !tbaa !13
  %760 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %753, i64 noundef %184) #11
  %761 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %760, i64 noundef 1, i64 noundef 1)
  store double %733, ptr %761, align 8, !tbaa !13
  %762 = load double, ptr %88, align 8, !tbaa !13
  %763 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %753, i64 noundef %184) #11
  %764 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %763, i64 noundef 2, i64 noundef 0)
  store double %762, ptr %764, align 8, !tbaa !13
  %765 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %753, i64 noundef %184) #11
  %766 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %765, i64 noundef 2, i64 noundef 2)
  store double 1.000000e+00, ptr %766, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %101) #11
  %767 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %768 = getelementptr inbounds i8, ptr %767, i64 16
  %769 = load i32, ptr %768, align 8, !tbaa !12
  %770 = zext i32 %769 to i64
  %771 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %770)
  %772 = load double, ptr %771, align 8, !tbaa !13
  store double %772, ptr %101, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %102) #11
  %773 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %774 = getelementptr inbounds i8, ptr %773, i64 16
  %775 = load i32, ptr %774, align 8, !tbaa !12
  %776 = add i32 %775, 1
  %777 = zext i32 %776 to i64
  %778 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %777)
  %779 = load double, ptr %778, align 8, !tbaa !13
  store double %779, ptr %102, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %103) #11
  %780 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %781 = getelementptr inbounds i8, ptr %780, i64 16
  %782 = load i32, ptr %781, align 8, !tbaa !12
  %783 = add i32 %782, 2
  %784 = zext i32 %783 to i64
  %785 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %784)
  %786 = load double, ptr %785, align 8, !tbaa !13
  store double %786, ptr %103, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %104) #11
  %787 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %753, i64 noundef %184) #11
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %105) #11
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %105, ptr noundef nonnull align 8 dereferenceable(8) %101, ptr noundef nonnull align 8 dereferenceable(8) %102, ptr noundef nonnull align 8 dereferenceable(8) %103)
  %788 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEEEmlINS1_IdLi3ELi1ELi0ELi3ELi1EEEEEKNS_7ProductIS2_T_Li0EEERKNS0_IS7_EE(ptr noundef nonnull align 1 dereferenceable(1) %787, ptr noundef nonnull align 1 dereferenceable(1) %105)
  %789 = extractvalue { ptr, ptr } %788, 0
  store ptr %789, ptr %104, align 8
  %790 = getelementptr inbounds i8, ptr %104, i64 8
  %791 = extractvalue { ptr, ptr } %788, 1
  store ptr %791, ptr %790, align 8
  %792 = getelementptr inbounds i8, ptr %0, i64 208
  %793 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %792, i64 noundef %184) #11
  %794 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen7ProductINS1_6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS3_IdLi3ELi1ELi0ELi3ELi1EEELi0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48) %793, ptr noundef nonnull align 1 dereferenceable(1) %104)
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %105) #11
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %104) #11
  %795 = getelementptr inbounds i8, ptr %0, i64 232
  %796 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %795, i64 noundef %184) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %106) #11
  %797 = load double, ptr %103, align 8, !tbaa !13
  %798 = fmul double %743, %797
  %799 = load double, ptr %101, align 8, !tbaa !13
  %800 = load double, ptr %88, align 8, !tbaa !13
  %801 = load double, ptr %102, align 8, !tbaa !13
  %802 = fmul double %800, %738
  %803 = fmul double %802, %801
  %804 = fmul double %799, %803
  %805 = call double @llvm.fmuladd.f64(double %798, double %799, double %804)
  %806 = fmul double %733, %797
  %807 = call double @llvm.fmuladd.f64(double %806, double %801, double %805)
  store double %807, ptr %106, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %107) #11
  %808 = fmul double %731, %738
  %809 = fmul double %808, %797
  %810 = fmul double %732, %800
  %811 = fmul double %810, %801
  %812 = fmul double %799, %811
  %813 = call double @llvm.fmuladd.f64(double %809, double %799, double %812)
  %814 = fmul double %797, %742
  %815 = call double @llvm.fmuladd.f64(double %814, double %801, double %813)
  store double %815, ptr %107, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %108) #11
  %816 = fmul double %731, %801
  %817 = fmul double %799, %816
  store double %817, ptr %108, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %109) #11
  store double 0.000000e+00, ptr %109, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %110) #11
  store double 0.000000e+00, ptr %110, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %111) #11
  store double 0.000000e+00, ptr %111, align 8, !tbaa !13
  call void @_ZN15SpatialVector_t3setERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %796, ptr noundef nonnull align 8 dereferenceable(8) %106, ptr noundef nonnull align 8 dereferenceable(8) %107, ptr noundef nonnull align 8 dereferenceable(8) %108, ptr noundef nonnull align 8 dereferenceable(8) %109, ptr noundef nonnull align 8 dereferenceable(8) %110, ptr noundef nonnull align 8 dereferenceable(8) %111)
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %111) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %110) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %109) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %108) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %107) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %106) #11
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %112) #11
  %818 = getelementptr inbounds i8, ptr %0, i64 280
  %819 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %818, i64 noundef %184) #11
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %112, ptr noundef nonnull align 8 dereferenceable(96) %89, ptr noundef nonnull align 8 dereferenceable(96) %819)
  %820 = getelementptr inbounds i8, ptr %0, i64 720
  %821 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %820, i64 noundef %184) #11
  %822 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %821, ptr noundef nonnull align 8 dereferenceable(96) %112) #11
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %112) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %103) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %102) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %101) #11
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %89) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %88) #11
  br label %1164

823:                                              ; preds = %703
  %824 = getelementptr inbounds i8, ptr %707, i64 8
  %825 = load i32, ptr %824, align 8, !tbaa !5
  %826 = icmp eq i32 %825, 9
  %827 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  br i1 %826, label %828, label %940

828:                                              ; preds = %823
  %829 = getelementptr inbounds i8, ptr %827, i64 16
  %830 = load i32, ptr %829, align 8, !tbaa !12
  %831 = zext i32 %830 to i64
  %832 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %831)
  %833 = load double, ptr %832, align 8, !tbaa !13
  %834 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %835 = getelementptr inbounds i8, ptr %834, i64 16
  %836 = load i32, ptr %835, align 8, !tbaa !12
  %837 = add i32 %836, 1
  %838 = zext i32 %837 to i64
  %839 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %838)
  %840 = load double, ptr %839, align 8, !tbaa !13
  %841 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %842 = getelementptr inbounds i8, ptr %841, i64 16
  %843 = load i32, ptr %842, align 8, !tbaa !12
  %844 = add i32 %843, 2
  %845 = zext i32 %844 to i64
  %846 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %845)
  %847 = load double, ptr %846, align 8, !tbaa !13
  %848 = tail call double @sin(double noundef %833) #11
  %849 = tail call double @cos(double noundef %833) #11
  %850 = tail call double @sin(double noundef %840) #11
  %851 = tail call double @cos(double noundef %840) #11
  %852 = tail call double @sin(double noundef %847) #11
  %853 = tail call double @cos(double noundef %847) #11
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %113) #11
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %114) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %115) #11
  %854 = fmul double %850, %852
  %855 = fmul double %848, %854
  %856 = tail call double @llvm.fmuladd.f64(double %853, double %849, double %855)
  store double %856, ptr %115, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %116) #11
  %857 = fmul double %851, %852
  store double %857, ptr %116, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %117) #11
  %858 = fneg double %853
  %859 = fmul double %849, %854
  %860 = tail call double @llvm.fmuladd.f64(double %858, double %848, double %859)
  store double %860, ptr %117, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %118) #11
  %861 = fneg double %852
  %862 = fmul double %850, %853
  %863 = fmul double %848, %862
  %864 = tail call double @llvm.fmuladd.f64(double %861, double %849, double %863)
  store double %864, ptr %118, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %119) #11
  %865 = fmul double %851, %853
  store double %865, ptr %119, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %120) #11
  %866 = fmul double %849, %862
  %867 = tail call double @llvm.fmuladd.f64(double %852, double %848, double %866)
  store double %867, ptr %120, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %121) #11
  %868 = fmul double %848, %851
  store double %868, ptr %121, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %122) #11
  %869 = fneg double %850
  store double %869, ptr %122, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %123) #11
  %870 = fmul double %849, %851
  store double %870, ptr %123, align 8, !tbaa !13
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %114, ptr noundef nonnull align 8 dereferenceable(8) %115, ptr noundef nonnull align 8 dereferenceable(8) %116, ptr noundef nonnull align 8 dereferenceable(8) %117, ptr noundef nonnull align 8 dereferenceable(8) %118, ptr noundef nonnull align 8 dereferenceable(8) %119, ptr noundef nonnull align 8 dereferenceable(8) %120, ptr noundef nonnull align 8 dereferenceable(8) %121, ptr noundef nonnull align 8 dereferenceable(8) %122, ptr noundef nonnull align 8 dereferenceable(8) %123)
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %124) #11
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %125) #11
  call void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE4ZeroEv(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseNullaryOp.149") align 8 %125)
  call void @_ZN9Vector3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %124, ptr noundef nonnull align 1 dereferenceable(1) %125)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %113, ptr noundef nonnull align 8 dereferenceable(72) %114, ptr noundef nonnull align 8 dereferenceable(24) %124)
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %125) #11
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %124) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %123) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %122) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %121) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %120) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %119) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %118) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %117) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %116) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %115) #11
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %114) #11
  %871 = getelementptr inbounds i8, ptr %0, i64 328
  %872 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %871, i64 noundef %184) #11
  %873 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %872, i64 noundef 0, i64 noundef 0)
  store double %857, ptr %873, align 8, !tbaa !13
  %874 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %871, i64 noundef %184) #11
  %875 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %874, i64 noundef 0, i64 noundef 1)
  store double %853, ptr %875, align 8, !tbaa !13
  %876 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %871, i64 noundef %184) #11
  %877 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %876, i64 noundef 1, i64 noundef 0)
  store double %865, ptr %877, align 8, !tbaa !13
  %878 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %871, i64 noundef %184) #11
  %879 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %878, i64 noundef 1, i64 noundef 1)
  store double %861, ptr %879, align 8, !tbaa !13
  %880 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %871, i64 noundef %184) #11
  %881 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %880, i64 noundef 2, i64 noundef 0)
  store double %869, ptr %881, align 8, !tbaa !13
  %882 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %871, i64 noundef %184) #11
  %883 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %882, i64 noundef 2, i64 noundef 2)
  store double 1.000000e+00, ptr %883, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %126) #11
  %884 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %885 = getelementptr inbounds i8, ptr %884, i64 16
  %886 = load i32, ptr %885, align 8, !tbaa !12
  %887 = zext i32 %886 to i64
  %888 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %887)
  %889 = load double, ptr %888, align 8, !tbaa !13
  store double %889, ptr %126, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %127) #11
  %890 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %891 = getelementptr inbounds i8, ptr %890, i64 16
  %892 = load i32, ptr %891, align 8, !tbaa !12
  %893 = add i32 %892, 1
  %894 = zext i32 %893 to i64
  %895 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %894)
  %896 = load double, ptr %895, align 8, !tbaa !13
  store double %896, ptr %127, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %128) #11
  %897 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %898 = getelementptr inbounds i8, ptr %897, i64 16
  %899 = load i32, ptr %898, align 8, !tbaa !12
  %900 = add i32 %899, 2
  %901 = zext i32 %900 to i64
  %902 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %901)
  %903 = load double, ptr %902, align 8, !tbaa !13
  store double %903, ptr %128, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %129) #11
  %904 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %871, i64 noundef %184) #11
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %130) #11
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %130, ptr noundef nonnull align 8 dereferenceable(8) %126, ptr noundef nonnull align 8 dereferenceable(8) %127, ptr noundef nonnull align 8 dereferenceable(8) %128)
  %905 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEEEmlINS1_IdLi3ELi1ELi0ELi3ELi1EEEEEKNS_7ProductIS2_T_Li0EEERKNS0_IS7_EE(ptr noundef nonnull align 1 dereferenceable(1) %904, ptr noundef nonnull align 1 dereferenceable(1) %130)
  %906 = extractvalue { ptr, ptr } %905, 0
  store ptr %906, ptr %129, align 8
  %907 = getelementptr inbounds i8, ptr %129, i64 8
  %908 = extractvalue { ptr, ptr } %905, 1
  store ptr %908, ptr %907, align 8
  %909 = getelementptr inbounds i8, ptr %0, i64 208
  %910 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %909, i64 noundef %184) #11
  %911 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen7ProductINS1_6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS3_IdLi3ELi1ELi0ELi3ELi1EEELi0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48) %910, ptr noundef nonnull align 1 dereferenceable(1) %129)
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %130) #11
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %129) #11
  %912 = getelementptr inbounds i8, ptr %0, i64 232
  %913 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %912, i64 noundef %184) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %131) #11
  %914 = load double, ptr %128, align 8, !tbaa !13
  %915 = fmul double %865, %914
  %916 = load double, ptr %126, align 8, !tbaa !13
  %917 = load double, ptr %127, align 8, !tbaa !13
  %918 = fneg double %854
  %919 = fmul double %917, %918
  %920 = fmul double %916, %919
  %921 = call double @llvm.fmuladd.f64(double %915, double %916, double %920)
  %922 = fmul double %914, %861
  %923 = call double @llvm.fmuladd.f64(double %922, double %917, double %921)
  store double %923, ptr %131, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %132) #11
  %924 = fmul double %851, %861
  %925 = fmul double %924, %914
  %926 = fneg double %862
  %927 = fmul double %917, %926
  %928 = fmul double %916, %927
  %929 = call double @llvm.fmuladd.f64(double %925, double %916, double %928)
  %930 = fmul double %914, %858
  %931 = call double @llvm.fmuladd.f64(double %930, double %917, double %929)
  store double %931, ptr %132, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %133) #11
  %932 = fneg double %851
  %933 = fmul double %917, %932
  %934 = fmul double %916, %933
  store double %934, ptr %133, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %134) #11
  store double 0.000000e+00, ptr %134, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %135) #11
  store double 0.000000e+00, ptr %135, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %136) #11
  store double 0.000000e+00, ptr %136, align 8, !tbaa !13
  call void @_ZN15SpatialVector_t3setERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %913, ptr noundef nonnull align 8 dereferenceable(8) %131, ptr noundef nonnull align 8 dereferenceable(8) %132, ptr noundef nonnull align 8 dereferenceable(8) %133, ptr noundef nonnull align 8 dereferenceable(8) %134, ptr noundef nonnull align 8 dereferenceable(8) %135, ptr noundef nonnull align 8 dereferenceable(8) %136)
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %136) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %135) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %134) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %133) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %132) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %131) #11
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %137) #11
  %935 = getelementptr inbounds i8, ptr %0, i64 280
  %936 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %935, i64 noundef %184) #11
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %137, ptr noundef nonnull align 8 dereferenceable(96) %113, ptr noundef nonnull align 8 dereferenceable(96) %936)
  %937 = getelementptr inbounds i8, ptr %0, i64 720
  %938 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %937, i64 noundef %184) #11
  %939 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %938, ptr noundef nonnull align 8 dereferenceable(96) %137) #11
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %137) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %128) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %127) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %126) #11
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %113) #11
  br label %1164

940:                                              ; preds = %823
  %941 = getelementptr inbounds i8, ptr %827, i64 8
  %942 = load i32, ptr %941, align 8, !tbaa !5
  %943 = icmp eq i32 %942, 10
  %944 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  br i1 %943, label %945, label %1061

945:                                              ; preds = %940
  %946 = getelementptr inbounds i8, ptr %944, i64 16
  %947 = load i32, ptr %946, align 8, !tbaa !12
  %948 = zext i32 %947 to i64
  %949 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %948)
  %950 = load double, ptr %949, align 8, !tbaa !13
  %951 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %952 = getelementptr inbounds i8, ptr %951, i64 16
  %953 = load i32, ptr %952, align 8, !tbaa !12
  %954 = add i32 %953, 1
  %955 = zext i32 %954 to i64
  %956 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %955)
  %957 = load double, ptr %956, align 8, !tbaa !13
  %958 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %959 = getelementptr inbounds i8, ptr %958, i64 16
  %960 = load i32, ptr %959, align 8, !tbaa !12
  %961 = add i32 %960, 2
  %962 = zext i32 %961 to i64
  %963 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %962)
  %964 = load double, ptr %963, align 8, !tbaa !13
  %965 = tail call double @sin(double noundef %950) #11
  %966 = tail call double @cos(double noundef %950) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %138) #11
  %967 = tail call double @sin(double noundef %957) #11
  store double %967, ptr %138, align 8, !tbaa !13
  %968 = tail call double @cos(double noundef %957) #11
  %969 = tail call double @sin(double noundef %964) #11
  %970 = tail call double @cos(double noundef %964) #11
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %139) #11
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %140) #11
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %141) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %142) #11
  %971 = fneg double %965
  %972 = fmul double %967, %971
  %973 = fmul double %966, %970
  %974 = tail call double @llvm.fmuladd.f64(double %972, double %969, double %973)
  store double %974, ptr %142, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %143) #11
  %975 = fmul double %969, %967
  %976 = fmul double %966, %975
  %977 = tail call double @llvm.fmuladd.f64(double %965, double %970, double %976)
  store double %977, ptr %143, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %144) #11
  %978 = fneg double %969
  %979 = fmul double %968, %978
  store double %979, ptr %144, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %145) #11
  %980 = fmul double %968, %971
  store double %980, ptr %145, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %146) #11
  %981 = fmul double %966, %968
  store double %981, ptr %146, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %147) #11
  %982 = fmul double %965, %967
  %983 = fmul double %966, %969
  %984 = tail call double @llvm.fmuladd.f64(double %982, double %970, double %983)
  store double %984, ptr %147, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %148) #11
  %985 = fneg double %967
  %986 = fmul double %966, %985
  %987 = fmul double %970, %986
  %988 = tail call double @llvm.fmuladd.f64(double %965, double %969, double %987)
  store double %988, ptr %148, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %149) #11
  %989 = fmul double %968, %970
  store double %989, ptr %149, align 8, !tbaa !13
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %141, ptr noundef nonnull align 8 dereferenceable(8) %142, ptr noundef nonnull align 8 dereferenceable(8) %143, ptr noundef nonnull align 8 dereferenceable(8) %144, ptr noundef nonnull align 8 dereferenceable(8) %145, ptr noundef nonnull align 8 dereferenceable(8) %146, ptr noundef nonnull align 8 dereferenceable(8) %138, ptr noundef nonnull align 8 dereferenceable(8) %147, ptr noundef nonnull align 8 dereferenceable(8) %148, ptr noundef nonnull align 8 dereferenceable(8) %149)
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %150) #11
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %151) #11
  call void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE4ZeroEv(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseNullaryOp.149") align 8 %151)
  call void @_ZN9Vector3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %150, ptr noundef nonnull align 1 dereferenceable(1) %151)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %140, ptr noundef nonnull align 8 dereferenceable(72) %141, ptr noundef nonnull align 8 dereferenceable(24) %150)
  %990 = getelementptr inbounds i8, ptr %0, i64 280
  %991 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %990, i64 noundef %184) #11
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %139, ptr noundef nonnull align 8 dereferenceable(96) %140, ptr noundef nonnull align 8 dereferenceable(96) %991)
  %992 = getelementptr inbounds i8, ptr %0, i64 720
  %993 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %992, i64 noundef %184) #11
  %994 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %993, ptr noundef nonnull align 8 dereferenceable(96) %139) #11
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %151) #11
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %150) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %149) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %148) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %147) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %146) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %145) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %144) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %143) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %142) #11
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %141) #11
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %140) #11
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %139) #11
  %995 = getelementptr inbounds i8, ptr %0, i64 328
  %996 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %995, i64 noundef %184) #11
  %997 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %996, i64 noundef 0, i64 noundef 0)
  store double %979, ptr %997, align 8, !tbaa !13
  %998 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %995, i64 noundef %184) #11
  %999 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %998, i64 noundef 0, i64 noundef 1)
  store double %970, ptr %999, align 8, !tbaa !13
  %1000 = load double, ptr %138, align 8, !tbaa !13
  %1001 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %995, i64 noundef %184) #11
  %1002 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1001, i64 noundef 1, i64 noundef 0)
  store double %1000, ptr %1002, align 8, !tbaa !13
  %1003 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %995, i64 noundef %184) #11
  %1004 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1003, i64 noundef 1, i64 noundef 2)
  store double 1.000000e+00, ptr %1004, align 8, !tbaa !13
  %1005 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %995, i64 noundef %184) #11
  %1006 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1005, i64 noundef 2, i64 noundef 0)
  store double %989, ptr %1006, align 8, !tbaa !13
  %1007 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %995, i64 noundef %184) #11
  %1008 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1007, i64 noundef 2, i64 noundef 1)
  store double %969, ptr %1008, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %152) #11
  %1009 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %1010 = getelementptr inbounds i8, ptr %1009, i64 16
  %1011 = load i32, ptr %1010, align 8, !tbaa !12
  %1012 = zext i32 %1011 to i64
  %1013 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %1012)
  %1014 = load double, ptr %1013, align 8, !tbaa !13
  store double %1014, ptr %152, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %153) #11
  %1015 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %1016 = getelementptr inbounds i8, ptr %1015, i64 16
  %1017 = load i32, ptr %1016, align 8, !tbaa !12
  %1018 = add i32 %1017, 1
  %1019 = zext i32 %1018 to i64
  %1020 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %1019)
  %1021 = load double, ptr %1020, align 8, !tbaa !13
  store double %1021, ptr %153, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %154) #11
  %1022 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %1023 = getelementptr inbounds i8, ptr %1022, i64 16
  %1024 = load i32, ptr %1023, align 8, !tbaa !12
  %1025 = add i32 %1024, 2
  %1026 = zext i32 %1025 to i64
  %1027 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %1026)
  %1028 = load double, ptr %1027, align 8, !tbaa !13
  store double %1028, ptr %154, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %155) #11
  %1029 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %995, i64 noundef %184) #11
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %156) #11
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %156, ptr noundef nonnull align 8 dereferenceable(8) %152, ptr noundef nonnull align 8 dereferenceable(8) %153, ptr noundef nonnull align 8 dereferenceable(8) %154)
  %1030 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEEEmlINS1_IdLi3ELi1ELi0ELi3ELi1EEEEEKNS_7ProductIS2_T_Li0EEERKNS0_IS7_EE(ptr noundef nonnull align 1 dereferenceable(1) %1029, ptr noundef nonnull align 1 dereferenceable(1) %156)
  %1031 = extractvalue { ptr, ptr } %1030, 0
  store ptr %1031, ptr %155, align 8
  %1032 = getelementptr inbounds i8, ptr %155, i64 8
  %1033 = extractvalue { ptr, ptr } %1030, 1
  store ptr %1033, ptr %1032, align 8
  %1034 = getelementptr inbounds i8, ptr %0, i64 208
  %1035 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1034, i64 noundef %184) #11
  %1036 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen7ProductINS1_6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS3_IdLi3ELi1ELi0ELi3ELi1EEELi0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48) %1035, ptr noundef nonnull align 1 dereferenceable(1) %155)
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %156) #11
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %155) #11
  %1037 = getelementptr inbounds i8, ptr %0, i64 232
  %1038 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1037, i64 noundef %184) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %157) #11
  %1039 = fneg double %968
  %1040 = fmul double %970, %1039
  %1041 = load double, ptr %154, align 8, !tbaa !13
  %1042 = load double, ptr %138, align 8, !tbaa !13
  %1043 = fmul double %969, %1042
  %1044 = load double, ptr %153, align 8, !tbaa !13
  %1045 = fmul double %1043, %1044
  %1046 = call double @llvm.fmuladd.f64(double %1040, double %1041, double %1045)
  %1047 = load double, ptr %152, align 8, !tbaa !13
  %1048 = fmul double %1044, %978
  %1049 = fmul double %1041, %1048
  %1050 = call double @llvm.fmuladd.f64(double %1046, double %1047, double %1049)
  store double %1050, ptr %157, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %158) #11
  %1051 = fmul double %968, %1044
  %1052 = fmul double %1051, %1047
  store double %1052, ptr %158, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %159) #11
  %1053 = fneg double %1042
  %1054 = fmul double %970, %1053
  %1055 = fmul double %969, %1039
  %1056 = fmul double %1055, %1041
  %1057 = call double @llvm.fmuladd.f64(double %1054, double %1044, double %1056)
  %1058 = fmul double %970, %1041
  %1059 = fmul double %1058, %1044
  %1060 = call double @llvm.fmuladd.f64(double %1057, double %1047, double %1059)
  store double %1060, ptr %159, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %160) #11
  store double 0.000000e+00, ptr %160, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %161) #11
  store double 0.000000e+00, ptr %161, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %162) #11
  store double 0.000000e+00, ptr %162, align 8, !tbaa !13
  call void @_ZN15SpatialVector_t3setERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %1038, ptr noundef nonnull align 8 dereferenceable(8) %157, ptr noundef nonnull align 8 dereferenceable(8) %158, ptr noundef nonnull align 8 dereferenceable(8) %159, ptr noundef nonnull align 8 dereferenceable(8) %160, ptr noundef nonnull align 8 dereferenceable(8) %161, ptr noundef nonnull align 8 dereferenceable(8) %162)
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %162) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %161) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %160) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %159) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %158) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %157) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %154) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %153) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %152) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %138) #11
  br label %1164

1061:                                             ; preds = %940
  %1062 = getelementptr inbounds i8, ptr %944, i64 8
  %1063 = load i32, ptr %1062, align 8, !tbaa !5
  %1064 = icmp eq i32 %1063, 11
  br i1 %1064, label %1065, label %1139

1065:                                             ; preds = %1061
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %163) #11
  %1066 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %1067 = getelementptr inbounds i8, ptr %1066, i64 16
  %1068 = load i32, ptr %1067, align 8, !tbaa !12
  %1069 = zext i32 %1068 to i64
  %1070 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %1069)
  %1071 = load double, ptr %1070, align 8, !tbaa !13
  store double %1071, ptr %163, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %164) #11
  %1072 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %1073 = getelementptr inbounds i8, ptr %1072, i64 16
  %1074 = load i32, ptr %1073, align 8, !tbaa !12
  %1075 = add i32 %1074, 1
  %1076 = zext i32 %1075 to i64
  %1077 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %1076)
  %1078 = load double, ptr %1077, align 8, !tbaa !13
  store double %1078, ptr %164, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %165) #11
  %1079 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %1080 = getelementptr inbounds i8, ptr %1079, i64 16
  %1081 = load i32, ptr %1080, align 8, !tbaa !12
  %1082 = add i32 %1081, 2
  %1083 = zext i32 %1082 to i64
  %1084 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %1083)
  %1085 = load double, ptr %1084, align 8, !tbaa !13
  store double %1085, ptr %165, align 8, !tbaa !13
  %1086 = getelementptr inbounds i8, ptr %0, i64 328
  %1087 = tail call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1086, i64 noundef %184) #11
  %1088 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1087, i64 noundef 3, i64 noundef 0)
  store double 1.000000e+00, ptr %1088, align 8, !tbaa !13
  %1089 = tail call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1086, i64 noundef %184) #11
  %1090 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1089, i64 noundef 4, i64 noundef 1)
  store double 1.000000e+00, ptr %1090, align 8, !tbaa !13
  %1091 = tail call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1086, i64 noundef %184) #11
  %1092 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1091, i64 noundef 5, i64 noundef 2)
  store double 1.000000e+00, ptr %1092, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %166) #11
  %1093 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %1094 = getelementptr inbounds i8, ptr %1093, i64 16
  %1095 = load i32, ptr %1094, align 8, !tbaa !12
  %1096 = zext i32 %1095 to i64
  %1097 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %1096)
  %1098 = load double, ptr %1097, align 8, !tbaa !13
  store double %1098, ptr %166, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %167) #11
  %1099 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %1100 = getelementptr inbounds i8, ptr %1099, i64 16
  %1101 = load i32, ptr %1100, align 8, !tbaa !12
  %1102 = add i32 %1101, 1
  %1103 = zext i32 %1102 to i64
  %1104 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %1103)
  %1105 = load double, ptr %1104, align 8, !tbaa !13
  store double %1105, ptr %167, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %168) #11
  %1106 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %1107 = getelementptr inbounds i8, ptr %1106, i64 16
  %1108 = load i32, ptr %1107, align 8, !tbaa !12
  %1109 = add i32 %1108, 2
  %1110 = zext i32 %1109 to i64
  %1111 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %3, i64 noundef %1110)
  %1112 = load double, ptr %1111, align 8, !tbaa !13
  store double %1112, ptr %168, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %169) #11
  %1113 = tail call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1086, i64 noundef %184) #11
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %170) #11
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %170, ptr noundef nonnull align 8 dereferenceable(8) %166, ptr noundef nonnull align 8 dereferenceable(8) %167, ptr noundef nonnull align 8 dereferenceable(8) %168)
  %1114 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEEEmlINS1_IdLi3ELi1ELi0ELi3ELi1EEEEEKNS_7ProductIS2_T_Li0EEERKNS0_IS7_EE(ptr noundef nonnull align 1 dereferenceable(1) %1113, ptr noundef nonnull align 1 dereferenceable(1) %170)
  %1115 = extractvalue { ptr, ptr } %1114, 0
  store ptr %1115, ptr %169, align 8
  %1116 = getelementptr inbounds i8, ptr %169, i64 8
  %1117 = extractvalue { ptr, ptr } %1114, 1
  store ptr %1117, ptr %1116, align 8
  %1118 = getelementptr inbounds i8, ptr %0, i64 208
  %1119 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1118, i64 noundef %184) #11
  %1120 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen7ProductINS1_6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS3_IdLi3ELi1ELi0ELi3ELi1EEELi0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48) %1119, ptr noundef nonnull align 1 dereferenceable(1) %169)
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %170) #11
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %169) #11
  %1121 = getelementptr inbounds i8, ptr %0, i64 232
  %1122 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1121, i64 noundef %184) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %171) #11
  store double 0.000000e+00, ptr %171, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %172) #11
  store double 0.000000e+00, ptr %172, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %173) #11
  store double 0.000000e+00, ptr %173, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %174) #11
  store double 0.000000e+00, ptr %174, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %175) #11
  store double 0.000000e+00, ptr %175, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %176) #11
  store double 0.000000e+00, ptr %176, align 8, !tbaa !13
  call void @_ZN15SpatialVector_t3setERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %1122, ptr noundef nonnull align 8 dereferenceable(8) %171, ptr noundef nonnull align 8 dereferenceable(8) %172, ptr noundef nonnull align 8 dereferenceable(8) %173, ptr noundef nonnull align 8 dereferenceable(8) %174, ptr noundef nonnull align 8 dereferenceable(8) %175, ptr noundef nonnull align 8 dereferenceable(8) %176)
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %176) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %175) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %174) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %173) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %172) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %171) #11
  %1123 = getelementptr inbounds i8, ptr %0, i64 280
  %1124 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1123, i64 noundef %184) #11
  %1125 = getelementptr inbounds i8, ptr %0, i64 720
  %1126 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1125, i64 noundef %184) #11
  %1127 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSERKS_(ptr noundef nonnull align 8 dereferenceable(72) %1126, ptr noundef nonnull align 8 dereferenceable(72) %1124)
  call void @llvm.lifetime.start.p0(i64 32, ptr nonnull %177) #11
  %1128 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1123, i64 noundef %184) #11
  %1129 = getelementptr inbounds i8, ptr %1128, i64 72
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %178) #11
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %179) #11
  %1130 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1123, i64 noundef %184) #11
  %1131 = call ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1) %1130)
  store ptr %1131, ptr %179, align 8
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %180) #11
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %180, ptr noundef nonnull align 8 dereferenceable(8) %163, ptr noundef nonnull align 8 dereferenceable(8) %164, ptr noundef nonnull align 8 dereferenceable(8) %165)
  %1132 = call { ptr, ptr } @_ZNK5Eigen10MatrixBaseINS_9TransposeINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEmlINS2_IdLi3ELi1ELi0ELi3ELi1EEEEEKNS_7ProductIS4_T_Li0EEERKNS0_IS9_EE(ptr noundef nonnull align 1 dereferenceable(1) %179, ptr noundef nonnull align 1 dereferenceable(1) %180)
  %1133 = extractvalue { ptr, ptr } %1132, 0
  store ptr %1133, ptr %178, align 8
  %1134 = getelementptr inbounds i8, ptr %178, i64 8
  %1135 = extractvalue { ptr, ptr } %1132, 1
  store ptr %1135, ptr %1134, align 8
  call void @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEplINS_7ProductINS_9TransposeINS1_IdLi3ELi3ELi0ELi3ELi3EEEEES2_Li0EEEEEKNS_13CwiseBinaryOpINS_8internal13scalar_sum_opIdNSB_6traitsIT_E6ScalarEEEKS2_KSE_EERKNS0_ISE_EE(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseBinaryOp.161") align 8 %177, ptr noundef nonnull align 1 dereferenceable(1) %1129, ptr noundef nonnull align 1 dereferenceable(1) %178)
  %1136 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1125, i64 noundef %184) #11
  %1137 = getelementptr inbounds i8, ptr %1136, i64 72
  %1138 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSIN5Eigen13CwiseBinaryOpINS1_8internal13scalar_sum_opIddEEKNS1_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEKNS1_7ProductINS1_9TransposeINS6_IdLi3ELi3ELi0ELi3ELi3EEEEES7_Li0EEEEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %1137, ptr noundef nonnull align 1 dereferenceable(1) %177)
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %180) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %179) #11
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %178) #11
  call void @llvm.lifetime.end.p0(i64 32, ptr nonnull %177) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %168) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %167) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %166) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %165) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %164) #11
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %163) #11
  br label %1164

1139:                                             ; preds = %1061
  %1140 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %1141 = getelementptr inbounds i8, ptr %1140, i64 8
  %1142 = load i32, ptr %1141, align 8, !tbaa !5
  %1143 = icmp eq i32 %1142, 21
  br i1 %1143, label %1144, label %1155

1144:                                             ; preds = %1139
  %1145 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %1146 = getelementptr inbounds i8, ptr %0, i64 448
  %1147 = getelementptr inbounds i8, ptr %1145, i64 20
  %1148 = load i32, ptr %1147, align 4, !tbaa !17
  %1149 = sext i32 %1148 to i64
  %1150 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSt6vectorIPN17RigidBodyDynamics11CustomJointESaIS2_EEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1146, i64 noundef %1149) #11
  %1151 = load ptr, ptr %1150, align 8, !tbaa !18
  %1152 = load ptr, ptr %1151, align 8, !tbaa !19
  %1153 = getelementptr inbounds i8, ptr %1152, i64 16
  %1154 = load ptr, ptr %1153, align 8
  tail call void %1154(ptr noundef nonnull align 8 dereferenceable(216) %1151, ptr noundef nonnull align 8 dereferenceable(872) %0, i32 noundef %1, ptr noundef nonnull align 8 dereferenceable(16) %2, ptr noundef nonnull align 8 dereferenceable(16) %3)
  br label %1164

1155:                                             ; preds = %1139
  %1156 = 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.5)
  %1157 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %184) #11
  %1158 = getelementptr inbounds i8, ptr %1157, i64 8
  %1159 = load i32, ptr %1158, align 8, !tbaa !5
  %1160 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEi(ptr noundef nonnull align 8 dereferenceable(8) %1156, i32 noundef %1159)
  %1161 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) %1160, ptr noundef nonnull @.str.6)
  %1162 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEj(ptr noundef nonnull align 8 dereferenceable(8) %1161, i32 noundef %1)
  %1163 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %1162, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
  tail call void @abort() #10
  unreachable

1164:                                             ; preds = %288, %486, %547, %708, %945, %1144, %1065, %828, %589, %526, %387, %189
  ret void
}

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

; 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 #2 align 2

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

; Function Attrs: nounwind
declare void @sincos(double noundef, ptr noundef, ptr noundef) local_unnamed_addr #4

; 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 #5 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 #2 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 #5 align 2

; Function Attrs: mustprogress nocallback nofree nosync nounwind speculatable willreturn memory(none)
declare double @llvm.fmuladd.f64(double, double, double) #6

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

; Function Attrs: inlinehint mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(72), ptr noundef nonnull align 8 dereferenceable(72)) local_unnamed_addr #7 align 2

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

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSERKS_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(24)) local_unnamed_addr #5 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 #2 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 #5 align 2

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

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

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

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen13CwiseBinaryOpINS1_8internal17scalar_product_opIddEEKNS1_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEKNS1_14CwiseNullaryOpINS3_18scalar_constant_opIdEES8_EEEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEE5blockIiiEENS3_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, i32 noundef) local_unnamed_addr #5 align 2

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

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

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

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

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

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

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

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

; Function Attrs: inlinehint mustprogress nounwind uwtable
declare dso_local noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSEOS_(ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 16 dereferenceable(48)) local_unnamed_addr #7 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind noalias writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8, ptr noundef nonnull align 8 dereferenceable(96), ptr noundef nonnull align 8 dereferenceable(96)) local_unnamed_addr #0 align 2

; Function Attrs: inlinehint mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96), ptr noundef nonnull align 8 dereferenceable(96)) local_unnamed_addr #7 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 #0 align 2

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

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

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96), ptr noundef nonnull align 8 dereferenceable(72), ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #0 align 2

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

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

; Function Attrs: mustprogress nofree nounwind willreturn memory(write)
declare double @sin(double noundef) local_unnamed_addr #8

; Function Attrs: mustprogress nofree nounwind willreturn memory(write)
declare double @cos(double noundef) local_unnamed_addr #8

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE4ZeroEv(ptr dead_on_unwind noalias writable sret(%"class.Eigen::CwiseNullaryOp.149") align 8) local_unnamed_addr #5 align 2

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

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

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSIN5Eigen7ProductINS1_6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS3_IdLi3ELi1ELi0ELi3ELi1EEELi0EEEEERS_RKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #0 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN15SpatialVector_t3setERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8)) local_unnamed_addr #0 align 2

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

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

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

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

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

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZNSt6vectorIPN17RigidBodyDynamics11CustomJointESaIS2_EEixEm(ptr noundef nonnull align 8 dereferenceable(24), i64 noundef) local_unnamed_addr #2 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 #5

declare noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEi(ptr noundef nonnull align 8 dereferenceable(8), i32 noundef) local_unnamed_addr #9

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

; 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 #0 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)) #5

; Function Attrs: noreturn nounwind
declare void @abort() local_unnamed_addr #1

attributes #0 = { 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 #1 = { 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 #2 = { 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 #3 = { mustprogress nocallback nofree nosync nounwind willreturn memory(argmem: readwrite) }
attributes #4 = { 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 #5 = { 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 #6 = { mustprogress nocallback nofree nosync nounwind speculatable willreturn memory(none) }
attributes #7 = { 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 #8 = { mustprogress nofree nounwind willreturn memory(write) "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 #9 = { "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 #10 = { noreturn nounwind }
attributes #11 = { 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, !10, i64 8}
!6 = !{!"_ZTSN17RigidBodyDynamics5JointE", !7, i64 0, !10, i64 8, !11, i64 12, !11, i64 16, !11, i64 20}
!7 = !{!"any pointer", !8, i64 0}
!8 = !{!"omnipotent char", !9, i64 0}
!9 = !{!"Simple C++ TBAA"}
!10 = !{!"_ZTSN17RigidBodyDynamics9JointTypeE", !8, i64 0}
!11 = !{!"int", !8, i64 0}
!12 = !{!6, !11, i64 16}
!13 = !{!14, !14, i64 0}
!14 = !{!"double", !8, i64 0}
!15 = !{!6, !7, i64 0}
!16 = !{!6, !11, i64 12}
!17 = !{!6, !11, i64 20}
!18 = !{!7, !7, i64 0}
!19 = !{!20, !20, i64 0}
!20 = !{!"vtable pointer", !9, i64 0}


; Effective O3 passes: EarlyCSEPass,GVNPass,GlobalOptPass,InferFunctionAttrsPass,InstCombinePass,ReassociatePass,SROAPass,SimplifyCFGPass,TailCallElimPass