; 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 }
%"struct.RigidBodyDynamics::Math::SpatialTransform" = type { %class.Matrix3_t, %class.Vector3_t }
%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] }
%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::Product" = type { ptr, %"class.Eigen::Block" }
%"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.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" }
%"struct.Eigen::internal::scalar_constant_op" = type { double }
%"class.Eigen::CwiseNullaryOp.190" = type { [3 x i8] }
%"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
@_ZSt4cerr = external global %"class.std::basic_ostream", align 8
@__PRETTY_FUNCTION__._ZN17RigidBodyDynamics16jcalc_X_lambda_SERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE = external hidden unnamed_addr constant [82 x i8], align 1
@.str.8 = external hidden unnamed_addr constant [27 x i8], align 1

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

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

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

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

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

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

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

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

; 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)) #4 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) #1 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) #4 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)) #7

; Function Attrs: mustprogress uwtable
define dso_local 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) #7 {
  %4 = alloca ptr, align 8
  %5 = alloca i32, align 4
  %6 = alloca ptr, align 8
  %7 = alloca double, align 8
  %8 = alloca double, align 8
  %9 = alloca %class.Matrix3_t, 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 double, align 8
  %17 = alloca double, align 8
  %18 = alloca %class.Matrix3_t, 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 double, align 8
  %26 = alloca double, align 8
  %27 = alloca %class.Matrix3_t, align 8
  %28 = alloca double, align 8
  %29 = alloca double, align 8
  %30 = alloca double, align 8
  %31 = alloca double, align 8
  %32 = alloca double, align 8
  %33 = alloca double, align 8
  %34 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %35 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %36 = alloca %class.Vector3_t, align 8
  %37 = alloca %"class.Eigen::Product", align 8
  %38 = alloca %"class.Eigen::Block", align 8
  %39 = alloca %class.SpatialVector_t, align 16
  %40 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %41 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %42 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %43 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %44 = alloca %class.Matrix3_t, align 8
  %45 = alloca %"class.RigidBodyDynamics::Math::Quaternion", align 16
  %46 = alloca %class.Vector3_t, align 8
  %47 = alloca double, align 8
  %48 = alloca double, align 8
  %49 = alloca double, align 8
  %50 = alloca double, align 8
  %51 = alloca double, align 8
  %52 = alloca double, align 8
  %53 = alloca double, align 8
  %54 = alloca double, align 8
  %55 = alloca double, align 8
  %56 = alloca double, align 8
  %57 = alloca double, align 8
  %58 = alloca double, align 8
  %59 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %60 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %61 = alloca %class.Matrix3_t, align 8
  %62 = alloca double, align 8
  %63 = alloca double, align 8
  %64 = alloca double, 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 %class.Vector3_t, align 8
  %72 = alloca double, align 8
  %73 = alloca double, align 8
  %74 = alloca double, align 8
  %75 = alloca double, align 8
  %76 = alloca double, align 8
  %77 = alloca double, 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 %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %85 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %86 = alloca %class.Matrix3_t, align 8
  %87 = alloca double, align 8
  %88 = alloca double, align 8
  %89 = alloca double, align 8
  %90 = alloca double, align 8
  %91 = alloca double, align 8
  %92 = alloca double, align 8
  %93 = alloca double, align 8
  %94 = alloca double, align 8
  %95 = alloca %class.Vector3_t, align 8
  %96 = alloca double, align 8
  %97 = alloca double, align 8
  %98 = alloca double, align 8
  %99 = alloca double, align 8
  %100 = alloca double, 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 %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %109 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %110 = alloca %class.Matrix3_t, align 8
  %111 = alloca double, 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 double, align 8
  %122 = alloca double, align 8
  %123 = alloca double, align 8
  %124 = alloca double, align 8
  %125 = alloca double, align 8
  %126 = alloca double, 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 %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %135 = alloca %class.Matrix3_t, 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 double, align 8
  %144 = alloca %class.Vector3_t, align 8
  %145 = alloca %"class.Eigen::CwiseNullaryOp.149", align 8
  %146 = alloca double, align 8
  %147 = alloca double, align 8
  %148 = alloca double, align 8
  %149 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %150 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %151 = alloca %class.Matrix3_t, align 8
  %152 = alloca %"class.Eigen::CwiseNullaryOp.190", align 1
  %153 = alloca %class.Vector3_t, align 8
  %154 = alloca ptr, align 8
  %155 = alloca ptr, align 8
  store ptr %0, ptr %4, align 8, !tbaa !5
  store i32 %1, ptr %5, align 4, !tbaa !9
  store ptr %2, ptr %6, align 8, !tbaa !5
  %156 = load i32, ptr %5, align 4, !tbaa !9
  %157 = icmp ugt i32 %156, 0
  br i1 %157, label %158, label %159

158:                                              ; preds = %3
  br label %161

159:                                              ; preds = %3
  call void @__assert_fail(ptr noundef @.str, ptr noundef @.str.4, i32 noundef 385, ptr noundef @__PRETTY_FUNCTION__._ZN17RigidBodyDynamics16jcalc_X_lambda_SERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE) #8
  unreachable

160:                                              ; No predecessors!
  br label %161

161:                                              ; preds = %160, %158
  %162 = load ptr, ptr %4, align 8, !tbaa !5
  %163 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %162, i32 0, i32 10
  %164 = load i32, ptr %5, align 4, !tbaa !9
  %165 = zext i32 %164 to i64
  %166 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %163, i64 noundef %165) #9
  %167 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %166, i32 0, i32 1
  %168 = load i32, ptr %167, align 8, !tbaa !11
  %169 = icmp eq i32 %168, 3
  br i1 %169, label %170, label %352

170:                                              ; preds = %161
  call void @llvm.lifetime.start.p0(i64 8, ptr %7) #9
  call void @llvm.lifetime.start.p0(i64 8, ptr %8) #9
  %171 = load ptr, ptr %6, align 8, !tbaa !5
  %172 = load ptr, ptr %4, align 8, !tbaa !5
  %173 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %172, i32 0, i32 10
  %174 = load i32, ptr %5, align 4, !tbaa !9
  %175 = zext i32 %174 to i64
  %176 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %173, i64 noundef %175) #9
  %177 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %176, i32 0, i32 3
  %178 = load i32, ptr %177, align 8, !tbaa !14
  %179 = zext i32 %178 to i64
  %180 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %171, i64 noundef %179)
  %181 = load double, ptr %180, align 8, !tbaa !15
  call void @sincos(double noundef %181, ptr noundef %7, ptr noundef %8) #9
  call void @llvm.lifetime.start.p0(i64 72, ptr %9) #9
  %182 = load ptr, ptr %4, align 8, !tbaa !5
  %183 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %182, i32 0, i32 15
  %184 = load i32, ptr %5, align 4, !tbaa !9
  %185 = zext i32 %184 to i64
  %186 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %183, i64 noundef %185) #9
  %187 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %186, i32 0, i32 0
  %188 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %187, i64 noundef 0, i64 noundef 0)
  %189 = load ptr, ptr %4, align 8, !tbaa !5
  %190 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %189, i32 0, i32 15
  %191 = load i32, ptr %5, align 4, !tbaa !9
  %192 = zext i32 %191 to i64
  %193 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %190, i64 noundef %192) #9
  %194 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %193, i32 0, i32 0
  %195 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %194, i64 noundef 0, i64 noundef 1)
  %196 = load ptr, ptr %4, align 8, !tbaa !5
  %197 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %196, i32 0, i32 15
  %198 = load i32, ptr %5, align 4, !tbaa !9
  %199 = zext i32 %198 to i64
  %200 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %197, i64 noundef %199) #9
  %201 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %200, i32 0, i32 0
  %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 %10) #9
  %203 = load double, ptr %8, align 8, !tbaa !15
  %204 = load ptr, ptr %4, align 8, !tbaa !5
  %205 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %204, i32 0, i32 15
  %206 = load i32, ptr %5, align 4, !tbaa !9
  %207 = zext i32 %206 to i64
  %208 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %205, i64 noundef %207) #9
  %209 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %208, i32 0, i32 0
  %210 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %209, i64 noundef 1, i64 noundef 0)
  %211 = load double, ptr %210, align 8, !tbaa !15
  %212 = load double, ptr %7, align 8, !tbaa !15
  %213 = load ptr, ptr %4, align 8, !tbaa !5
  %214 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %213, i32 0, i32 15
  %215 = load i32, ptr %5, align 4, !tbaa !9
  %216 = zext i32 %215 to i64
  %217 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %214, i64 noundef %216) #9
  %218 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %217, i32 0, i32 0
  %219 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %218, i64 noundef 2, i64 noundef 0)
  %220 = load double, ptr %219, align 8, !tbaa !15
  %221 = fmul double %212, %220
  %222 = call double @llvm.fmuladd.f64(double %203, double %211, double %221)
  store double %222, ptr %10, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %11) #9
  %223 = load double, ptr %8, align 8, !tbaa !15
  %224 = load ptr, ptr %4, align 8, !tbaa !5
  %225 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %224, i32 0, i32 15
  %226 = load i32, ptr %5, align 4, !tbaa !9
  %227 = zext i32 %226 to i64
  %228 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %225, i64 noundef %227) #9
  %229 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %228, i32 0, i32 0
  %230 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %229, i64 noundef 1, i64 noundef 1)
  %231 = load double, ptr %230, align 8, !tbaa !15
  %232 = load double, ptr %7, align 8, !tbaa !15
  %233 = load ptr, ptr %4, align 8, !tbaa !5
  %234 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %233, i32 0, i32 15
  %235 = load i32, ptr %5, align 4, !tbaa !9
  %236 = zext i32 %235 to i64
  %237 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %234, i64 noundef %236) #9
  %238 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %237, i32 0, i32 0
  %239 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %238, i64 noundef 2, i64 noundef 1)
  %240 = load double, ptr %239, align 8, !tbaa !15
  %241 = fmul double %232, %240
  %242 = call double @llvm.fmuladd.f64(double %223, double %231, double %241)
  store double %242, ptr %11, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %12) #9
  %243 = load double, ptr %8, align 8, !tbaa !15
  %244 = load ptr, ptr %4, align 8, !tbaa !5
  %245 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %244, i32 0, i32 15
  %246 = load i32, ptr %5, align 4, !tbaa !9
  %247 = zext i32 %246 to i64
  %248 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %245, i64 noundef %247) #9
  %249 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %248, i32 0, i32 0
  %250 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %249, i64 noundef 1, i64 noundef 2)
  %251 = load double, ptr %250, align 8, !tbaa !15
  %252 = load double, ptr %7, align 8, !tbaa !15
  %253 = load ptr, ptr %4, align 8, !tbaa !5
  %254 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %253, i32 0, i32 15
  %255 = load i32, ptr %5, align 4, !tbaa !9
  %256 = zext i32 %255 to i64
  %257 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %254, i64 noundef %256) #9
  %258 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %257, i32 0, i32 0
  %259 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %258, i64 noundef 2, i64 noundef 2)
  %260 = load double, ptr %259, align 8, !tbaa !15
  %261 = fmul double %252, %260
  %262 = call double @llvm.fmuladd.f64(double %243, double %251, double %261)
  store double %262, ptr %12, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %13) #9
  %263 = load double, ptr %7, align 8, !tbaa !15
  %264 = fneg double %263
  %265 = load ptr, ptr %4, align 8, !tbaa !5
  %266 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %265, i32 0, i32 15
  %267 = load i32, ptr %5, align 4, !tbaa !9
  %268 = zext i32 %267 to i64
  %269 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %266, i64 noundef %268) #9
  %270 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %269, i32 0, i32 0
  %271 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %270, i64 noundef 1, i64 noundef 0)
  %272 = load double, ptr %271, align 8, !tbaa !15
  %273 = load double, ptr %8, align 8, !tbaa !15
  %274 = load ptr, ptr %4, align 8, !tbaa !5
  %275 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %274, i32 0, i32 15
  %276 = load i32, ptr %5, align 4, !tbaa !9
  %277 = zext i32 %276 to i64
  %278 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %275, i64 noundef %277) #9
  %279 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %278, i32 0, i32 0
  %280 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %279, i64 noundef 2, i64 noundef 0)
  %281 = load double, ptr %280, align 8, !tbaa !15
  %282 = fmul double %273, %281
  %283 = call double @llvm.fmuladd.f64(double %264, double %272, double %282)
  store double %283, ptr %13, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %14) #9
  %284 = load double, ptr %7, align 8, !tbaa !15
  %285 = fneg double %284
  %286 = load ptr, ptr %4, align 8, !tbaa !5
  %287 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %286, i32 0, i32 15
  %288 = load i32, ptr %5, align 4, !tbaa !9
  %289 = zext i32 %288 to i64
  %290 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %287, i64 noundef %289) #9
  %291 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %290, i32 0, i32 0
  %292 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %291, i64 noundef 1, i64 noundef 1)
  %293 = load double, ptr %292, align 8, !tbaa !15
  %294 = load double, ptr %8, align 8, !tbaa !15
  %295 = load ptr, ptr %4, align 8, !tbaa !5
  %296 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %295, i32 0, i32 15
  %297 = load i32, ptr %5, align 4, !tbaa !9
  %298 = zext i32 %297 to i64
  %299 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %296, i64 noundef %298) #9
  %300 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %299, i32 0, i32 0
  %301 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %300, i64 noundef 2, i64 noundef 1)
  %302 = load double, ptr %301, align 8, !tbaa !15
  %303 = fmul double %294, %302
  %304 = call double @llvm.fmuladd.f64(double %285, double %293, double %303)
  store double %304, ptr %14, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %15) #9
  %305 = load double, ptr %7, align 8, !tbaa !15
  %306 = fneg double %305
  %307 = load ptr, ptr %4, align 8, !tbaa !5
  %308 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %307, i32 0, i32 15
  %309 = load i32, ptr %5, align 4, !tbaa !9
  %310 = zext i32 %309 to i64
  %311 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %308, i64 noundef %310) #9
  %312 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %311, i32 0, i32 0
  %313 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %312, i64 noundef 1, i64 noundef 2)
  %314 = load double, ptr %313, align 8, !tbaa !15
  %315 = load double, ptr %8, align 8, !tbaa !15
  %316 = load ptr, ptr %4, align 8, !tbaa !5
  %317 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %316, i32 0, i32 15
  %318 = load i32, ptr %5, align 4, !tbaa !9
  %319 = zext i32 %318 to i64
  %320 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %317, i64 noundef %319) #9
  %321 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %320, i32 0, i32 0
  %322 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %321, i64 noundef 2, i64 noundef 2)
  %323 = load double, ptr %322, align 8, !tbaa !15
  %324 = fmul double %315, %323
  %325 = call double @llvm.fmuladd.f64(double %306, double %314, double %324)
  store double %325, ptr %15, align 8, !tbaa !15
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %9, ptr noundef nonnull align 8 dereferenceable(8) %188, ptr noundef nonnull align 8 dereferenceable(8) %195, ptr noundef nonnull align 8 dereferenceable(8) %202, 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, ptr noundef nonnull align 8 dereferenceable(8) %14, ptr noundef nonnull align 8 dereferenceable(8) %15)
  %326 = load ptr, ptr %4, align 8, !tbaa !5
  %327 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %326, i32 0, i32 34
  %328 = load i32, ptr %5, align 4, !tbaa !9
  %329 = zext i32 %328 to i64
  %330 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %327, i64 noundef %329) #9
  %331 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %330, i32 0, i32 0
  %332 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(72) %331, ptr noundef nonnull align 8 dereferenceable(72) %9) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %15) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %14) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %13) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %12) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %11) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %10) #9
  call void @llvm.lifetime.end.p0(i64 72, ptr %9) #9
  %333 = load ptr, ptr %4, align 8, !tbaa !5
  %334 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %333, i32 0, i32 15
  %335 = load i32, ptr %5, align 4, !tbaa !9
  %336 = zext i32 %335 to i64
  %337 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %334, i64 noundef %336) #9
  %338 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %337, i32 0, i32 1
  %339 = load ptr, ptr %4, align 8, !tbaa !5
  %340 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %339, i32 0, i32 34
  %341 = load i32, ptr %5, 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) #9
  %344 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %343, i32 0, i32 1
  %345 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSERKS_(ptr noundef nonnull align 8 dereferenceable(24) %344, ptr noundef nonnull align 8 dereferenceable(24) %338)
  %346 = load ptr, ptr %4, align 8, !tbaa !5
  %347 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %346, i32 0, i32 11
  %348 = load i32, ptr %5, align 4, !tbaa !9
  %349 = zext i32 %348 to i64
  %350 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %347, i64 noundef %349) #9
  %351 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %350, i64 noundef 0)
  store double 1.000000e+00, ptr %351, align 8, !tbaa !15
  call void @llvm.lifetime.end.p0(i64 8, ptr %8) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %7) #9
  br label %1660

352:                                              ; preds = %161
  %353 = load ptr, ptr %4, align 8, !tbaa !5
  %354 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %353, i32 0, i32 10
  %355 = load i32, ptr %5, align 4, !tbaa !9
  %356 = zext i32 %355 to i64
  %357 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %354, i64 noundef %356) #9
  %358 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %357, i32 0, i32 1
  %359 = load i32, ptr %358, align 8, !tbaa !11
  %360 = icmp eq i32 %359, 4
  br i1 %360, label %361, label %543

361:                                              ; preds = %352
  call void @llvm.lifetime.start.p0(i64 8, ptr %16) #9
  call void @llvm.lifetime.start.p0(i64 8, ptr %17) #9
  %362 = load ptr, ptr %6, align 8, !tbaa !5
  %363 = load ptr, ptr %4, align 8, !tbaa !5
  %364 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %363, i32 0, i32 10
  %365 = load i32, ptr %5, align 4, !tbaa !9
  %366 = zext i32 %365 to i64
  %367 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %364, i64 noundef %366) #9
  %368 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %367, i32 0, i32 3
  %369 = load i32, ptr %368, align 8, !tbaa !14
  %370 = zext i32 %369 to i64
  %371 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %362, i64 noundef %370)
  %372 = load double, ptr %371, align 8, !tbaa !15
  call void @sincos(double noundef %372, ptr noundef %16, ptr noundef %17) #9
  call void @llvm.lifetime.start.p0(i64 72, ptr %18) #9
  call void @llvm.lifetime.start.p0(i64 8, ptr %19) #9
  %373 = load double, ptr %17, align 8, !tbaa !15
  %374 = load ptr, ptr %4, align 8, !tbaa !5
  %375 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %374, i32 0, i32 15
  %376 = load i32, ptr %5, align 4, !tbaa !9
  %377 = zext i32 %376 to i64
  %378 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %375, i64 noundef %377) #9
  %379 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %378, i32 0, i32 0
  %380 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %379, i64 noundef 0, i64 noundef 0)
  %381 = load double, ptr %380, align 8, !tbaa !15
  %382 = load double, ptr %16, align 8, !tbaa !15
  %383 = fneg double %382
  %384 = load ptr, ptr %4, align 8, !tbaa !5
  %385 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %384, i32 0, i32 15
  %386 = load i32, ptr %5, align 4, !tbaa !9
  %387 = zext i32 %386 to i64
  %388 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %385, i64 noundef %387) #9
  %389 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %388, i32 0, i32 0
  %390 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %389, i64 noundef 2, i64 noundef 0)
  %391 = load double, ptr %390, align 8, !tbaa !15
  %392 = fmul double %383, %391
  %393 = call double @llvm.fmuladd.f64(double %373, double %381, double %392)
  store double %393, ptr %19, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %20) #9
  %394 = load double, ptr %17, align 8, !tbaa !15
  %395 = load ptr, ptr %4, align 8, !tbaa !5
  %396 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %395, i32 0, i32 15
  %397 = load i32, ptr %5, align 4, !tbaa !9
  %398 = zext i32 %397 to i64
  %399 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %396, i64 noundef %398) #9
  %400 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %399, i32 0, i32 0
  %401 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %400, i64 noundef 0, i64 noundef 1)
  %402 = load double, ptr %401, align 8, !tbaa !15
  %403 = load double, ptr %16, align 8, !tbaa !15
  %404 = fneg double %403
  %405 = load ptr, ptr %4, align 8, !tbaa !5
  %406 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %405, i32 0, i32 15
  %407 = load i32, ptr %5, align 4, !tbaa !9
  %408 = zext i32 %407 to i64
  %409 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %406, i64 noundef %408) #9
  %410 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %409, i32 0, i32 0
  %411 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %410, i64 noundef 2, i64 noundef 1)
  %412 = load double, ptr %411, align 8, !tbaa !15
  %413 = fmul double %404, %412
  %414 = call double @llvm.fmuladd.f64(double %394, double %402, double %413)
  store double %414, ptr %20, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %21) #9
  %415 = load double, ptr %17, align 8, !tbaa !15
  %416 = load ptr, ptr %4, align 8, !tbaa !5
  %417 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %416, i32 0, i32 15
  %418 = load i32, ptr %5, align 4, !tbaa !9
  %419 = zext i32 %418 to i64
  %420 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %417, i64 noundef %419) #9
  %421 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %420, i32 0, i32 0
  %422 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %421, i64 noundef 0, i64 noundef 2)
  %423 = load double, ptr %422, align 8, !tbaa !15
  %424 = load double, ptr %16, align 8, !tbaa !15
  %425 = fneg double %424
  %426 = load ptr, ptr %4, align 8, !tbaa !5
  %427 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %426, i32 0, i32 15
  %428 = load i32, ptr %5, align 4, !tbaa !9
  %429 = zext i32 %428 to i64
  %430 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %427, i64 noundef %429) #9
  %431 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %430, i32 0, i32 0
  %432 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %431, i64 noundef 2, i64 noundef 2)
  %433 = load double, ptr %432, align 8, !tbaa !15
  %434 = fmul double %425, %433
  %435 = call double @llvm.fmuladd.f64(double %415, double %423, double %434)
  store double %435, ptr %21, align 8, !tbaa !15
  %436 = load ptr, ptr %4, align 8, !tbaa !5
  %437 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %436, i32 0, i32 15
  %438 = load i32, ptr %5, align 4, !tbaa !9
  %439 = zext i32 %438 to i64
  %440 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %437, i64 noundef %439) #9
  %441 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %440, i32 0, i32 0
  %442 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %441, i64 noundef 1, i64 noundef 0)
  %443 = load ptr, ptr %4, align 8, !tbaa !5
  %444 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %443, i32 0, i32 15
  %445 = load i32, ptr %5, align 4, !tbaa !9
  %446 = zext i32 %445 to i64
  %447 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %444, i64 noundef %446) #9
  %448 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %447, i32 0, i32 0
  %449 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %448, i64 noundef 1, i64 noundef 1)
  %450 = load ptr, ptr %4, align 8, !tbaa !5
  %451 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %450, i32 0, i32 15
  %452 = load i32, ptr %5, 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) #9
  %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 1, i64 noundef 2)
  call void @llvm.lifetime.start.p0(i64 8, ptr %22) #9
  %457 = load double, ptr %16, align 8, !tbaa !15
  %458 = load ptr, ptr %4, align 8, !tbaa !5
  %459 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %458, i32 0, i32 15
  %460 = load i32, ptr %5, align 4, !tbaa !9
  %461 = zext i32 %460 to i64
  %462 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %459, i64 noundef %461) #9
  %463 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %462, i32 0, i32 0
  %464 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %463, i64 noundef 0, i64 noundef 0)
  %465 = load double, ptr %464, align 8, !tbaa !15
  %466 = load double, ptr %17, align 8, !tbaa !15
  %467 = load ptr, ptr %4, align 8, !tbaa !5
  %468 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %467, i32 0, i32 15
  %469 = load i32, ptr %5, align 4, !tbaa !9
  %470 = zext i32 %469 to i64
  %471 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %468, i64 noundef %470) #9
  %472 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %471, i32 0, i32 0
  %473 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %472, i64 noundef 2, i64 noundef 0)
  %474 = load double, ptr %473, align 8, !tbaa !15
  %475 = fmul double %466, %474
  %476 = call double @llvm.fmuladd.f64(double %457, double %465, double %475)
  store double %476, ptr %22, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %23) #9
  %477 = load double, ptr %16, align 8, !tbaa !15
  %478 = load ptr, ptr %4, align 8, !tbaa !5
  %479 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %478, i32 0, i32 15
  %480 = load i32, ptr %5, align 4, !tbaa !9
  %481 = zext i32 %480 to i64
  %482 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %479, i64 noundef %481) #9
  %483 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %482, i32 0, i32 0
  %484 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %483, i64 noundef 0, i64 noundef 1)
  %485 = load double, ptr %484, align 8, !tbaa !15
  %486 = load double, ptr %17, align 8, !tbaa !15
  %487 = load ptr, ptr %4, align 8, !tbaa !5
  %488 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %487, i32 0, i32 15
  %489 = load i32, ptr %5, align 4, !tbaa !9
  %490 = zext i32 %489 to i64
  %491 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %488, i64 noundef %490) #9
  %492 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %491, i32 0, i32 0
  %493 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %492, i64 noundef 2, i64 noundef 1)
  %494 = load double, ptr %493, align 8, !tbaa !15
  %495 = fmul double %486, %494
  %496 = call double @llvm.fmuladd.f64(double %477, double %485, double %495)
  store double %496, ptr %23, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %24) #9
  %497 = load double, ptr %16, align 8, !tbaa !15
  %498 = load ptr, ptr %4, align 8, !tbaa !5
  %499 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %498, i32 0, i32 15
  %500 = load i32, ptr %5, align 4, !tbaa !9
  %501 = zext i32 %500 to i64
  %502 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %499, i64 noundef %501) #9
  %503 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %502, i32 0, i32 0
  %504 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %503, i64 noundef 0, i64 noundef 2)
  %505 = load double, ptr %504, align 8, !tbaa !15
  %506 = load double, ptr %17, align 8, !tbaa !15
  %507 = load ptr, ptr %4, align 8, !tbaa !5
  %508 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %507, i32 0, i32 15
  %509 = load i32, ptr %5, align 4, !tbaa !9
  %510 = zext i32 %509 to i64
  %511 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %508, i64 noundef %510) #9
  %512 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %511, i32 0, i32 0
  %513 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %512, i64 noundef 2, i64 noundef 2)
  %514 = load double, ptr %513, align 8, !tbaa !15
  %515 = fmul double %506, %514
  %516 = call double @llvm.fmuladd.f64(double %497, double %505, double %515)
  store double %516, ptr %24, align 8, !tbaa !15
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %18, ptr noundef nonnull align 8 dereferenceable(8) %19, ptr noundef nonnull align 8 dereferenceable(8) %20, ptr noundef nonnull align 8 dereferenceable(8) %21, ptr noundef nonnull align 8 dereferenceable(8) %442, ptr noundef nonnull align 8 dereferenceable(8) %449, ptr noundef nonnull align 8 dereferenceable(8) %456, ptr noundef nonnull align 8 dereferenceable(8) %22, ptr noundef nonnull align 8 dereferenceable(8) %23, ptr noundef nonnull align 8 dereferenceable(8) %24)
  %517 = load ptr, ptr %4, align 8, !tbaa !5
  %518 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %517, i32 0, i32 34
  %519 = load i32, ptr %5, align 4, !tbaa !9
  %520 = zext i32 %519 to i64
  %521 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %518, i64 noundef %520) #9
  %522 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %521, i32 0, i32 0
  %523 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(72) %522, ptr noundef nonnull align 8 dereferenceable(72) %18) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %24) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %23) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %22) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %21) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %20) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %19) #9
  call void @llvm.lifetime.end.p0(i64 72, ptr %18) #9
  %524 = load ptr, ptr %4, align 8, !tbaa !5
  %525 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %524, i32 0, i32 15
  %526 = load i32, ptr %5, align 4, !tbaa !9
  %527 = zext i32 %526 to i64
  %528 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %525, i64 noundef %527) #9
  %529 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %528, i32 0, i32 1
  %530 = load ptr, ptr %4, align 8, !tbaa !5
  %531 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %530, i32 0, i32 34
  %532 = load i32, ptr %5, align 4, !tbaa !9
  %533 = zext i32 %532 to i64
  %534 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %531, i64 noundef %533) #9
  %535 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %534, i32 0, i32 1
  %536 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSERKS_(ptr noundef nonnull align 8 dereferenceable(24) %535, ptr noundef nonnull align 8 dereferenceable(24) %529)
  %537 = load ptr, ptr %4, align 8, !tbaa !5
  %538 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %537, i32 0, i32 11
  %539 = load i32, ptr %5, align 4, !tbaa !9
  %540 = zext i32 %539 to i64
  %541 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %538, i64 noundef %540) #9
  %542 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %541, i64 noundef 1)
  store double 1.000000e+00, ptr %542, align 8, !tbaa !15
  call void @llvm.lifetime.end.p0(i64 8, ptr %17) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %16) #9
  br label %1659

543:                                              ; preds = %352
  %544 = load ptr, ptr %4, align 8, !tbaa !5
  %545 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %544, i32 0, i32 10
  %546 = load i32, ptr %5, align 4, !tbaa !9
  %547 = zext i32 %546 to i64
  %548 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %545, i64 noundef %547) #9
  %549 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %548, i32 0, i32 1
  %550 = load i32, ptr %549, align 8, !tbaa !11
  %551 = icmp eq i32 %550, 5
  br i1 %551, label %552, label %734

552:                                              ; preds = %543
  call void @llvm.lifetime.start.p0(i64 8, ptr %25) #9
  call void @llvm.lifetime.start.p0(i64 8, ptr %26) #9
  %553 = load ptr, ptr %6, align 8, !tbaa !5
  %554 = load ptr, ptr %4, align 8, !tbaa !5
  %555 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %554, i32 0, i32 10
  %556 = load i32, ptr %5, align 4, !tbaa !9
  %557 = zext i32 %556 to i64
  %558 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %555, i64 noundef %557) #9
  %559 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %558, i32 0, i32 3
  %560 = load i32, ptr %559, align 8, !tbaa !14
  %561 = zext i32 %560 to i64
  %562 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %553, i64 noundef %561)
  %563 = load double, ptr %562, align 8, !tbaa !15
  call void @sincos(double noundef %563, ptr noundef %25, ptr noundef %26) #9
  call void @llvm.lifetime.start.p0(i64 72, ptr %27) #9
  call void @llvm.lifetime.start.p0(i64 8, ptr %28) #9
  %564 = load double, ptr %26, align 8, !tbaa !15
  %565 = load ptr, ptr %4, align 8, !tbaa !5
  %566 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %565, i32 0, i32 15
  %567 = load i32, ptr %5, align 4, !tbaa !9
  %568 = zext i32 %567 to i64
  %569 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %566, i64 noundef %568) #9
  %570 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %569, i32 0, i32 0
  %571 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %570, i64 noundef 0, i64 noundef 0)
  %572 = load double, ptr %571, align 8, !tbaa !15
  %573 = load double, ptr %25, align 8, !tbaa !15
  %574 = load ptr, ptr %4, align 8, !tbaa !5
  %575 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %574, i32 0, i32 15
  %576 = load i32, ptr %5, 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) #9
  %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 1, i64 noundef 0)
  %581 = load double, ptr %580, align 8, !tbaa !15
  %582 = fmul double %573, %581
  %583 = call double @llvm.fmuladd.f64(double %564, double %572, double %582)
  store double %583, ptr %28, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %29) #9
  %584 = load double, ptr %26, align 8, !tbaa !15
  %585 = load ptr, ptr %4, align 8, !tbaa !5
  %586 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %585, i32 0, i32 15
  %587 = load i32, ptr %5, align 4, !tbaa !9
  %588 = zext i32 %587 to i64
  %589 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %586, i64 noundef %588) #9
  %590 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %589, i32 0, i32 0
  %591 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %590, i64 noundef 0, i64 noundef 1)
  %592 = load double, ptr %591, align 8, !tbaa !15
  %593 = load double, ptr %25, align 8, !tbaa !15
  %594 = load ptr, ptr %4, align 8, !tbaa !5
  %595 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %594, i32 0, i32 15
  %596 = load i32, ptr %5, align 4, !tbaa !9
  %597 = zext i32 %596 to i64
  %598 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %595, i64 noundef %597) #9
  %599 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %598, i32 0, i32 0
  %600 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %599, i64 noundef 1, i64 noundef 1)
  %601 = load double, ptr %600, align 8, !tbaa !15
  %602 = fmul double %593, %601
  %603 = call double @llvm.fmuladd.f64(double %584, double %592, double %602)
  store double %603, ptr %29, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %30) #9
  %604 = load double, ptr %26, align 8, !tbaa !15
  %605 = load ptr, ptr %4, align 8, !tbaa !5
  %606 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %605, i32 0, i32 15
  %607 = load i32, ptr %5, align 4, !tbaa !9
  %608 = zext i32 %607 to i64
  %609 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %606, i64 noundef %608) #9
  %610 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %609, i32 0, i32 0
  %611 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %610, i64 noundef 0, i64 noundef 2)
  %612 = load double, ptr %611, align 8, !tbaa !15
  %613 = load double, ptr %25, align 8, !tbaa !15
  %614 = load ptr, ptr %4, align 8, !tbaa !5
  %615 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %614, i32 0, i32 15
  %616 = load i32, ptr %5, align 4, !tbaa !9
  %617 = zext i32 %616 to i64
  %618 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %615, i64 noundef %617) #9
  %619 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %618, i32 0, i32 0
  %620 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %619, i64 noundef 1, i64 noundef 2)
  %621 = load double, ptr %620, align 8, !tbaa !15
  %622 = fmul double %613, %621
  %623 = call double @llvm.fmuladd.f64(double %604, double %612, double %622)
  store double %623, ptr %30, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %31) #9
  %624 = load double, ptr %25, align 8, !tbaa !15
  %625 = fneg double %624
  %626 = load ptr, ptr %4, align 8, !tbaa !5
  %627 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %626, i32 0, i32 15
  %628 = load i32, ptr %5, align 4, !tbaa !9
  %629 = zext i32 %628 to i64
  %630 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %627, i64 noundef %629) #9
  %631 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %630, i32 0, i32 0
  %632 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %631, i64 noundef 0, i64 noundef 0)
  %633 = load double, ptr %632, align 8, !tbaa !15
  %634 = load double, ptr %26, align 8, !tbaa !15
  %635 = load ptr, ptr %4, align 8, !tbaa !5
  %636 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %635, i32 0, i32 15
  %637 = load i32, ptr %5, align 4, !tbaa !9
  %638 = zext i32 %637 to i64
  %639 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %636, i64 noundef %638) #9
  %640 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %639, i32 0, i32 0
  %641 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %640, i64 noundef 1, i64 noundef 0)
  %642 = load double, ptr %641, align 8, !tbaa !15
  %643 = fmul double %634, %642
  %644 = call double @llvm.fmuladd.f64(double %625, double %633, double %643)
  store double %644, ptr %31, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %32) #9
  %645 = load double, ptr %25, align 8, !tbaa !15
  %646 = fneg double %645
  %647 = load ptr, ptr %4, align 8, !tbaa !5
  %648 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %647, i32 0, i32 15
  %649 = load i32, ptr %5, align 4, !tbaa !9
  %650 = zext i32 %649 to i64
  %651 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %648, i64 noundef %650) #9
  %652 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %651, i32 0, i32 0
  %653 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %652, i64 noundef 0, i64 noundef 1)
  %654 = load double, ptr %653, align 8, !tbaa !15
  %655 = load double, ptr %26, align 8, !tbaa !15
  %656 = load ptr, ptr %4, align 8, !tbaa !5
  %657 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %656, i32 0, i32 15
  %658 = load i32, ptr %5, align 4, !tbaa !9
  %659 = zext i32 %658 to i64
  %660 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %657, i64 noundef %659) #9
  %661 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %660, i32 0, i32 0
  %662 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %661, i64 noundef 1, i64 noundef 1)
  %663 = load double, ptr %662, align 8, !tbaa !15
  %664 = fmul double %655, %663
  %665 = call double @llvm.fmuladd.f64(double %646, double %654, double %664)
  store double %665, ptr %32, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %33) #9
  %666 = load double, ptr %25, align 8, !tbaa !15
  %667 = fneg double %666
  %668 = load ptr, ptr %4, align 8, !tbaa !5
  %669 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %668, i32 0, i32 15
  %670 = load i32, ptr %5, align 4, !tbaa !9
  %671 = zext i32 %670 to i64
  %672 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %669, i64 noundef %671) #9
  %673 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %672, i32 0, i32 0
  %674 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %673, i64 noundef 0, i64 noundef 2)
  %675 = load double, ptr %674, align 8, !tbaa !15
  %676 = load double, ptr %26, align 8, !tbaa !15
  %677 = load ptr, ptr %4, align 8, !tbaa !5
  %678 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %677, i32 0, i32 15
  %679 = load i32, ptr %5, align 4, !tbaa !9
  %680 = zext i32 %679 to i64
  %681 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %678, i64 noundef %680) #9
  %682 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %681, i32 0, i32 0
  %683 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %682, i64 noundef 1, i64 noundef 2)
  %684 = load double, ptr %683, align 8, !tbaa !15
  %685 = fmul double %676, %684
  %686 = call double @llvm.fmuladd.f64(double %667, double %675, double %685)
  store double %686, ptr %33, align 8, !tbaa !15
  %687 = load ptr, ptr %4, align 8, !tbaa !5
  %688 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %687, i32 0, i32 15
  %689 = load i32, ptr %5, align 4, !tbaa !9
  %690 = zext i32 %689 to i64
  %691 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %688, i64 noundef %690) #9
  %692 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %691, i32 0, i32 0
  %693 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %692, i64 noundef 2, i64 noundef 0)
  %694 = load ptr, ptr %4, align 8, !tbaa !5
  %695 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %694, i32 0, i32 15
  %696 = load i32, ptr %5, align 4, !tbaa !9
  %697 = zext i32 %696 to i64
  %698 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %695, i64 noundef %697) #9
  %699 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %698, i32 0, i32 0
  %700 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %699, i64 noundef 2, i64 noundef 1)
  %701 = load ptr, ptr %4, align 8, !tbaa !5
  %702 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %701, i32 0, i32 15
  %703 = load i32, ptr %5, 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) #9
  %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 2, i64 noundef 2)
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %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) %32, ptr noundef nonnull align 8 dereferenceable(8) %33, ptr noundef nonnull align 8 dereferenceable(8) %693, ptr noundef nonnull align 8 dereferenceable(8) %700, ptr noundef nonnull align 8 dereferenceable(8) %707)
  %708 = load ptr, ptr %4, align 8, !tbaa !5
  %709 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %708, i32 0, i32 34
  %710 = load i32, ptr %5, align 4, !tbaa !9
  %711 = zext i32 %710 to i64
  %712 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %709, i64 noundef %711) #9
  %713 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %712, i32 0, i32 0
  %714 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(72) %713, ptr noundef nonnull align 8 dereferenceable(72) %27) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %33) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %32) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %31) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %30) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %29) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %28) #9
  call void @llvm.lifetime.end.p0(i64 72, ptr %27) #9
  %715 = load ptr, ptr %4, align 8, !tbaa !5
  %716 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %715, i32 0, i32 15
  %717 = load i32, ptr %5, align 4, !tbaa !9
  %718 = zext i32 %717 to i64
  %719 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %716, i64 noundef %718) #9
  %720 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %719, i32 0, i32 1
  %721 = load ptr, ptr %4, align 8, !tbaa !5
  %722 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %721, i32 0, i32 34
  %723 = load i32, ptr %5, align 4, !tbaa !9
  %724 = zext i32 %723 to i64
  %725 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %722, i64 noundef %724) #9
  %726 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %725, i32 0, i32 1
  %727 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSERKS_(ptr noundef nonnull align 8 dereferenceable(24) %726, ptr noundef nonnull align 8 dereferenceable(24) %720)
  %728 = load ptr, ptr %4, align 8, !tbaa !5
  %729 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %728, i32 0, i32 11
  %730 = load i32, ptr %5, align 4, !tbaa !9
  %731 = zext i32 %730 to i64
  %732 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %729, i64 noundef %731) #9
  %733 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %732, i64 noundef 2)
  store double 1.000000e+00, ptr %733, align 8, !tbaa !15
  call void @llvm.lifetime.end.p0(i64 8, ptr %26) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %25) #9
  br label %1658

734:                                              ; preds = %543
  %735 = load ptr, ptr %4, align 8, !tbaa !5
  %736 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %735, i32 0, i32 10
  %737 = load i32, ptr %5, align 4, !tbaa !9
  %738 = zext i32 %737 to i64
  %739 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %736, i64 noundef %738) #9
  %740 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %739, i32 0, i32 1
  %741 = load i32, ptr %740, align 8, !tbaa !11
  %742 = icmp eq i32 %741, 14
  br i1 %742, label %743, label %803

743:                                              ; preds = %734
  call void @llvm.lifetime.start.p0(i64 96, ptr %34) #9
  %744 = load ptr, ptr %4, align 8, !tbaa !5
  %745 = load i32, ptr %5, align 4, !tbaa !9
  %746 = load ptr, ptr %6, align 8, !tbaa !5
  call void @_ZN17RigidBodyDynamics8jcalc_XJERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %34, ptr noundef nonnull align 8 dereferenceable(872) %744, i32 noundef %745, ptr noundef nonnull align 8 dereferenceable(16) %746)
  call void @llvm.lifetime.start.p0(i64 96, ptr %35) #9
  %747 = load ptr, ptr %4, align 8, !tbaa !5
  %748 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %747, i32 0, i32 15
  %749 = load i32, ptr %5, align 4, !tbaa !9
  %750 = zext i32 %749 to i64
  %751 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %748, i64 noundef %750) #9
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %35, ptr noundef nonnull align 8 dereferenceable(96) %34, ptr noundef nonnull align 8 dereferenceable(96) %751)
  %752 = load ptr, ptr %4, align 8, !tbaa !5
  %753 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %752, i32 0, i32 34
  %754 = load i32, ptr %5, align 4, !tbaa !9
  %755 = zext i32 %754 to i64
  %756 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %753, i64 noundef %755) #9
  %757 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %756, ptr noundef nonnull align 8 dereferenceable(96) %35) #9
  call void @llvm.lifetime.end.p0(i64 96, ptr %35) #9
  call void @llvm.lifetime.start.p0(i64 24, ptr %36) #9
  call void @llvm.lifetime.start.p0(i64 64, ptr %37) #9
  %758 = getelementptr inbounds %"struct.RigidBodyDynamics::Math::SpatialTransform", ptr %34, i32 0, i32 0
  call void @llvm.lifetime.start.p0(i64 56, ptr %38) #9
  %759 = load ptr, ptr %4, align 8, !tbaa !5
  %760 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %759, i32 0, i32 10
  %761 = load i32, ptr %5, align 4, !tbaa !9
  %762 = zext i32 %761 to i64
  %763 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %760, i64 noundef %762) #9
  %764 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %763, i32 0, i32 0
  %765 = load ptr, ptr %764, align 8, !tbaa !17
  %766 = getelementptr inbounds %class.SpatialVector_t, ptr %765, 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 %38, ptr noundef nonnull align 1 dereferenceable(1) %766, 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 %37, ptr noundef nonnull align 1 dereferenceable(1) %758, ptr noundef nonnull align 1 dereferenceable(1) %38)
  call void @_ZN9Vector3_tC2IN5Eigen7ProductINS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEENS1_5BlockINS3_IdLi6ELi1ELi0ELi6ELi1EEELin1ELin1ELb0EEELi0EEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %36, ptr noundef nonnull align 1 dereferenceable(1) %37)
  call void @llvm.lifetime.end.p0(i64 56, ptr %38) #9
  call void @llvm.lifetime.end.p0(i64 64, ptr %37) #9
  call void @llvm.lifetime.start.p0(i64 48, ptr %39) #9
  %767 = load ptr, ptr %4, align 8, !tbaa !5
  %768 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %767, i32 0, i32 10
  %769 = load i32, ptr %5, align 4, !tbaa !9
  %770 = zext i32 %769 to i64
  %771 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %768, i64 noundef %770) #9
  %772 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %771, i32 0, i32 0
  %773 = load ptr, ptr %772, align 8, !tbaa !17
  %774 = getelementptr inbounds %class.SpatialVector_t, ptr %773, i64 0
  %775 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %774, i64 noundef 0)
  %776 = load ptr, ptr %4, align 8, !tbaa !5
  %777 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %776, i32 0, i32 10
  %778 = load i32, ptr %5, align 4, !tbaa !9
  %779 = zext i32 %778 to i64
  %780 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %777, i64 noundef %779) #9
  %781 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %780, i32 0, i32 0
  %782 = load ptr, ptr %781, align 8, !tbaa !17
  %783 = getelementptr inbounds %class.SpatialVector_t, ptr %782, i64 0
  %784 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %783, i64 noundef 1)
  %785 = load ptr, ptr %4, align 8, !tbaa !5
  %786 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %785, i32 0, i32 10
  %787 = load i32, ptr %5, align 4, !tbaa !9
  %788 = zext i32 %787 to i64
  %789 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %786, i64 noundef %788) #9
  %790 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %789, i32 0, i32 0
  %791 = load ptr, ptr %790, align 8, !tbaa !17
  %792 = getelementptr inbounds %class.SpatialVector_t, ptr %791, i64 0
  %793 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %792, i64 noundef 2)
  %794 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %36, i64 noundef 0)
  %795 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %36, i64 noundef 1)
  %796 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %36, i64 noundef 2)
  call void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %39, ptr noundef nonnull align 8 dereferenceable(8) %775, ptr noundef nonnull align 8 dereferenceable(8) %784, ptr noundef nonnull align 8 dereferenceable(8) %793, ptr noundef nonnull align 8 dereferenceable(8) %794, ptr noundef nonnull align 8 dereferenceable(8) %795, ptr noundef nonnull align 8 dereferenceable(8) %796)
  %797 = load ptr, ptr %4, align 8, !tbaa !5
  %798 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %797, i32 0, i32 11
  %799 = load i32, ptr %5, align 4, !tbaa !9
  %800 = zext i32 %799 to i64
  %801 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %798, i64 noundef %800) #9
  %802 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSEOS_(ptr noundef nonnull align 16 dereferenceable(48) %801, ptr noundef nonnull align 16 dereferenceable(48) %39) #9
  call void @llvm.lifetime.end.p0(i64 48, ptr %39) #9
  call void @llvm.lifetime.end.p0(i64 24, ptr %36) #9
  call void @llvm.lifetime.end.p0(i64 96, ptr %34) #9
  br label %1657

803:                                              ; preds = %734
  %804 = load ptr, ptr %4, align 8, !tbaa !5
  %805 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %804, i32 0, i32 10
  %806 = load i32, ptr %5, align 4, !tbaa !9
  %807 = zext i32 %806 to i64
  %808 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %805, i64 noundef %807) #9
  %809 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %808, i32 0, i32 2
  %810 = load i32, ptr %809, align 4, !tbaa !18
  %811 = icmp eq i32 %810, 1
  br i1 %811, label %812, label %850

812:                                              ; preds = %803
  %813 = load ptr, ptr %4, align 8, !tbaa !5
  %814 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %813, i32 0, i32 10
  %815 = load i32, ptr %5, align 4, !tbaa !9
  %816 = zext i32 %815 to i64
  %817 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %814, i64 noundef %816) #9
  %818 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %817, i32 0, i32 1
  %819 = load i32, ptr %818, align 8, !tbaa !11
  %820 = icmp ne i32 %819, 21
  br i1 %820, label %821, label %850

821:                                              ; preds = %812
  call void @llvm.lifetime.start.p0(i64 96, ptr %40) #9
  call void @llvm.lifetime.start.p0(i64 96, ptr %41) #9
  %822 = load ptr, ptr %4, align 8, !tbaa !5
  %823 = load i32, ptr %5, align 4, !tbaa !9
  %824 = load ptr, ptr %6, align 8, !tbaa !5
  call void @_ZN17RigidBodyDynamics8jcalc_XJERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %41, ptr noundef nonnull align 8 dereferenceable(872) %822, i32 noundef %823, ptr noundef nonnull align 8 dereferenceable(16) %824)
  %825 = load ptr, ptr %4, align 8, !tbaa !5
  %826 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %825, i32 0, i32 15
  %827 = load i32, ptr %5, align 4, !tbaa !9
  %828 = zext i32 %827 to i64
  %829 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %826, i64 noundef %828) #9
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %40, ptr noundef nonnull align 8 dereferenceable(96) %41, ptr noundef nonnull align 8 dereferenceable(96) %829)
  %830 = load ptr, ptr %4, align 8, !tbaa !5
  %831 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %830, i32 0, i32 34
  %832 = load i32, ptr %5, align 4, !tbaa !9
  %833 = zext i32 %832 to i64
  %834 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %831, i64 noundef %833) #9
  %835 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %834, ptr noundef nonnull align 8 dereferenceable(96) %40) #9
  call void @llvm.lifetime.end.p0(i64 96, ptr %41) #9
  call void @llvm.lifetime.end.p0(i64 96, ptr %40) #9
  %836 = load ptr, ptr %4, align 8, !tbaa !5
  %837 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %836, i32 0, i32 10
  %838 = load i32, ptr %5, align 4, !tbaa !9
  %839 = zext i32 %838 to i64
  %840 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %837, i64 noundef %839) #9
  %841 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %840, i32 0, i32 0
  %842 = load ptr, ptr %841, align 8, !tbaa !17
  %843 = getelementptr inbounds %class.SpatialVector_t, ptr %842, i64 0
  %844 = load ptr, ptr %4, align 8, !tbaa !5
  %845 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %844, i32 0, i32 11
  %846 = load i32, ptr %5, align 4, !tbaa !9
  %847 = zext i32 %846 to i64
  %848 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %845, i64 noundef %847) #9
  %849 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSERKS_(ptr noundef nonnull align 16 dereferenceable(48) %848, ptr noundef nonnull align 16 dereferenceable(48) %843)
  br label %1656

850:                                              ; preds = %812, %803
  %851 = load ptr, ptr %4, align 8, !tbaa !5
  %852 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %851, i32 0, i32 10
  %853 = load i32, ptr %5, align 4, !tbaa !9
  %854 = zext i32 %853 to i64
  %855 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %852, i64 noundef %854) #9
  %856 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %855, i32 0, i32 1
  %857 = load i32, ptr %856, align 8, !tbaa !11
  %858 = icmp eq i32 %857, 6
  br i1 %858, label %859, label %892

859:                                              ; preds = %850
  call void @llvm.lifetime.start.p0(i64 96, ptr %42) #9
  call void @llvm.lifetime.start.p0(i64 96, ptr %43) #9
  call void @llvm.lifetime.start.p0(i64 72, ptr %44) #9
  call void @llvm.lifetime.start.p0(i64 32, ptr %45) #9
  %860 = load ptr, ptr %4, align 8, !tbaa !5
  %861 = load i32, ptr %5, align 4, !tbaa !9
  %862 = load ptr, ptr %6, align 8, !tbaa !5
  call void @_ZNK17RigidBodyDynamics5Model13GetQuaternionEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr dead_on_unwind writable sret(%"class.RigidBodyDynamics::Math::Quaternion") align 16 %45, ptr noundef nonnull align 8 dereferenceable(872) %860, i32 noundef %861, ptr noundef nonnull align 8 dereferenceable(16) %862)
  call void @_ZNK17RigidBodyDynamics4Math10Quaternion8toMatrixEv(ptr dead_on_unwind writable sret(%class.Matrix3_t) align 8 %44, ptr noundef nonnull align 16 dereferenceable(32) %45)
  call void @llvm.lifetime.start.p0(i64 24, ptr %46) #9
  call void @llvm.lifetime.start.p0(i64 8, ptr %47) #9
  store double 0.000000e+00, ptr %47, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %48) #9
  store double 0.000000e+00, ptr %48, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %49) #9
  store double 0.000000e+00, ptr %49, align 8, !tbaa !15
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %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)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %43, ptr noundef nonnull align 8 dereferenceable(72) %44, ptr noundef nonnull align 8 dereferenceable(24) %46)
  %863 = load ptr, ptr %4, align 8, !tbaa !5
  %864 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %863, i32 0, i32 15
  %865 = load i32, ptr %5, align 4, !tbaa !9
  %866 = zext i32 %865 to i64
  %867 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %864, i64 noundef %866) #9
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %42, ptr noundef nonnull align 8 dereferenceable(96) %43, ptr noundef nonnull align 8 dereferenceable(96) %867)
  %868 = load ptr, ptr %4, align 8, !tbaa !5
  %869 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %868, i32 0, i32 34
  %870 = load i32, ptr %5, align 4, !tbaa !9
  %871 = zext i32 %870 to i64
  %872 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %869, i64 noundef %871) #9
  %873 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %872, ptr noundef nonnull align 8 dereferenceable(96) %42) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %49) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %48) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %47) #9
  call void @llvm.lifetime.end.p0(i64 24, ptr %46) #9
  call void @llvm.lifetime.end.p0(i64 32, ptr %45) #9
  call void @llvm.lifetime.end.p0(i64 72, ptr %44) #9
  call void @llvm.lifetime.end.p0(i64 96, ptr %43) #9
  call void @llvm.lifetime.end.p0(i64 96, ptr %42) #9
  %874 = load ptr, ptr %4, align 8, !tbaa !5
  %875 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %874, i32 0, i32 17
  %876 = load i32, ptr %5, align 4, !tbaa !9
  %877 = zext i32 %876 to i64
  %878 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %875, i64 noundef %877) #9
  %879 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %878, i64 noundef 0, i64 noundef 0)
  store double 1.000000e+00, ptr %879, align 8, !tbaa !15
  %880 = load ptr, ptr %4, align 8, !tbaa !5
  %881 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %880, i32 0, i32 17
  %882 = load i32, ptr %5, align 4, !tbaa !9
  %883 = zext i32 %882 to i64
  %884 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %881, i64 noundef %883) #9
  %885 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %884, i64 noundef 1, i64 noundef 1)
  store double 1.000000e+00, ptr %885, align 8, !tbaa !15
  %886 = load ptr, ptr %4, align 8, !tbaa !5
  %887 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %886, i32 0, i32 17
  %888 = load i32, ptr %5, align 4, !tbaa !9
  %889 = zext i32 %888 to i64
  %890 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %887, i64 noundef %889) #9
  %891 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %890, i64 noundef 2, i64 noundef 2)
  store double 1.000000e+00, ptr %891, align 8, !tbaa !15
  br label %1655

892:                                              ; preds = %850
  %893 = load ptr, ptr %4, align 8, !tbaa !5
  %894 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %893, i32 0, i32 10
  %895 = load i32, ptr %5, align 4, !tbaa !9
  %896 = zext i32 %895 to i64
  %897 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %894, i64 noundef %896) #9
  %898 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %897, i32 0, i32 1
  %899 = load i32, ptr %898, align 8, !tbaa !11
  %900 = icmp eq i32 %899, 7
  br i1 %900, label %901, label %1055

901:                                              ; preds = %892
  call void @llvm.lifetime.start.p0(i64 8, ptr %50) #9
  %902 = load ptr, ptr %6, align 8, !tbaa !5
  %903 = load ptr, ptr %4, align 8, !tbaa !5
  %904 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %903, i32 0, i32 10
  %905 = load i32, ptr %5, align 4, !tbaa !9
  %906 = zext i32 %905 to i64
  %907 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %904, i64 noundef %906) #9
  %908 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %907, i32 0, i32 3
  %909 = load i32, ptr %908, align 8, !tbaa !14
  %910 = zext i32 %909 to i64
  %911 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %902, i64 noundef %910)
  %912 = load double, ptr %911, align 8, !tbaa !15
  store double %912, ptr %50, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %51) #9
  %913 = load ptr, ptr %6, align 8, !tbaa !5
  %914 = load ptr, ptr %4, align 8, !tbaa !5
  %915 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %914, i32 0, i32 10
  %916 = load i32, ptr %5, align 4, !tbaa !9
  %917 = zext i32 %916 to i64
  %918 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %915, i64 noundef %917) #9
  %919 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %918, i32 0, i32 3
  %920 = load i32, ptr %919, align 8, !tbaa !14
  %921 = add i32 %920, 1
  %922 = zext i32 %921 to i64
  %923 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %913, i64 noundef %922)
  %924 = load double, ptr %923, align 8, !tbaa !15
  store double %924, ptr %51, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %52) #9
  %925 = load ptr, ptr %6, align 8, !tbaa !5
  %926 = load ptr, ptr %4, align 8, !tbaa !5
  %927 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %926, i32 0, i32 10
  %928 = load i32, ptr %5, align 4, !tbaa !9
  %929 = zext i32 %928 to i64
  %930 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %927, i64 noundef %929) #9
  %931 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %930, i32 0, i32 3
  %932 = load i32, ptr %931, align 8, !tbaa !14
  %933 = add i32 %932, 2
  %934 = zext i32 %933 to i64
  %935 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %925, i64 noundef %934)
  %936 = load double, ptr %935, align 8, !tbaa !15
  store double %936, ptr %52, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %53) #9
  %937 = load double, ptr %50, align 8, !tbaa !15
  %938 = call double @sin(double noundef %937) #9
  store double %938, ptr %53, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %54) #9
  %939 = load double, ptr %50, align 8, !tbaa !15
  %940 = call double @cos(double noundef %939) #9
  store double %940, ptr %54, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %55) #9
  %941 = load double, ptr %51, align 8, !tbaa !15
  %942 = call double @sin(double noundef %941) #9
  store double %942, ptr %55, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %56) #9
  %943 = load double, ptr %51, align 8, !tbaa !15
  %944 = call double @cos(double noundef %943) #9
  store double %944, ptr %56, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %57) #9
  %945 = load double, ptr %52, align 8, !tbaa !15
  %946 = call double @sin(double noundef %945) #9
  store double %946, ptr %57, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %58) #9
  %947 = load double, ptr %52, align 8, !tbaa !15
  %948 = call double @cos(double noundef %947) #9
  store double %948, ptr %58, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 96, ptr %59) #9
  call void @llvm.lifetime.start.p0(i64 96, ptr %60) #9
  call void @llvm.lifetime.start.p0(i64 72, ptr %61) #9
  call void @llvm.lifetime.start.p0(i64 8, ptr %62) #9
  %949 = load double, ptr %54, align 8, !tbaa !15
  %950 = load double, ptr %56, align 8, !tbaa !15
  %951 = fmul double %949, %950
  store double %951, ptr %62, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %63) #9
  %952 = load double, ptr %53, align 8, !tbaa !15
  %953 = load double, ptr %56, align 8, !tbaa !15
  %954 = fmul double %952, %953
  store double %954, ptr %63, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %64) #9
  %955 = load double, ptr %55, align 8, !tbaa !15
  %956 = fneg double %955
  store double %956, ptr %64, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %65) #9
  %957 = load double, ptr %54, align 8, !tbaa !15
  %958 = load double, ptr %55, align 8, !tbaa !15
  %959 = fmul double %957, %958
  %960 = load double, ptr %57, align 8, !tbaa !15
  %961 = load double, ptr %53, align 8, !tbaa !15
  %962 = load double, ptr %58, align 8, !tbaa !15
  %963 = fmul double %961, %962
  %964 = fneg double %963
  %965 = call double @llvm.fmuladd.f64(double %959, double %960, double %964)
  store double %965, ptr %65, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %66) #9
  %966 = load double, ptr %53, align 8, !tbaa !15
  %967 = load double, ptr %55, align 8, !tbaa !15
  %968 = fmul double %966, %967
  %969 = load double, ptr %57, align 8, !tbaa !15
  %970 = load double, ptr %54, align 8, !tbaa !15
  %971 = load double, ptr %58, align 8, !tbaa !15
  %972 = fmul double %970, %971
  %973 = call double @llvm.fmuladd.f64(double %968, double %969, double %972)
  store double %973, ptr %66, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %67) #9
  %974 = load double, ptr %56, align 8, !tbaa !15
  %975 = load double, ptr %57, align 8, !tbaa !15
  %976 = fmul double %974, %975
  store double %976, ptr %67, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %68) #9
  %977 = load double, ptr %54, align 8, !tbaa !15
  %978 = load double, ptr %55, align 8, !tbaa !15
  %979 = fmul double %977, %978
  %980 = load double, ptr %58, align 8, !tbaa !15
  %981 = load double, ptr %53, align 8, !tbaa !15
  %982 = load double, ptr %57, align 8, !tbaa !15
  %983 = fmul double %981, %982
  %984 = call double @llvm.fmuladd.f64(double %979, double %980, double %983)
  store double %984, ptr %68, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %69) #9
  %985 = load double, ptr %53, align 8, !tbaa !15
  %986 = load double, ptr %55, align 8, !tbaa !15
  %987 = fmul double %985, %986
  %988 = load double, ptr %58, align 8, !tbaa !15
  %989 = load double, ptr %54, align 8, !tbaa !15
  %990 = load double, ptr %57, align 8, !tbaa !15
  %991 = fmul double %989, %990
  %992 = fneg double %991
  %993 = call double @llvm.fmuladd.f64(double %987, double %988, double %992)
  store double %993, ptr %69, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %70) #9
  %994 = load double, ptr %56, align 8, !tbaa !15
  %995 = load double, ptr %58, align 8, !tbaa !15
  %996 = fmul double %994, %995
  store double %996, ptr %70, align 8, !tbaa !15
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %61, ptr noundef nonnull align 8 dereferenceable(8) %62, ptr noundef nonnull align 8 dereferenceable(8) %63, ptr noundef nonnull align 8 dereferenceable(8) %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)
  call void @llvm.lifetime.start.p0(i64 24, ptr %71) #9
  call void @llvm.lifetime.start.p0(i64 8, ptr %72) #9
  store double 0.000000e+00, ptr %72, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %73) #9
  store double 0.000000e+00, ptr %73, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %74) #9
  store double 0.000000e+00, ptr %74, align 8, !tbaa !15
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %71, ptr noundef nonnull align 8 dereferenceable(8) %72, ptr noundef nonnull align 8 dereferenceable(8) %73, ptr noundef nonnull align 8 dereferenceable(8) %74)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %60, ptr noundef nonnull align 8 dereferenceable(72) %61, ptr noundef nonnull align 8 dereferenceable(24) %71)
  %997 = load ptr, ptr %4, align 8, !tbaa !5
  %998 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %997, i32 0, i32 15
  %999 = load i32, ptr %5, align 4, !tbaa !9
  %1000 = zext i32 %999 to i64
  %1001 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %998, i64 noundef %1000) #9
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %59, ptr noundef nonnull align 8 dereferenceable(96) %60, ptr noundef nonnull align 8 dereferenceable(96) %1001)
  %1002 = load ptr, ptr %4, align 8, !tbaa !5
  %1003 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1002, i32 0, i32 34
  %1004 = load i32, ptr %5, align 4, !tbaa !9
  %1005 = zext i32 %1004 to i64
  %1006 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1003, i64 noundef %1005) #9
  %1007 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %1006, ptr noundef nonnull align 8 dereferenceable(96) %59) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %74) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %73) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %72) #9
  call void @llvm.lifetime.end.p0(i64 24, ptr %71) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %70) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %69) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %68) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %67) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %66) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %65) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %64) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %63) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %62) #9
  call void @llvm.lifetime.end.p0(i64 72, ptr %61) #9
  call void @llvm.lifetime.end.p0(i64 96, ptr %60) #9
  call void @llvm.lifetime.end.p0(i64 96, ptr %59) #9
  %1008 = load double, ptr %55, align 8, !tbaa !15
  %1009 = fneg double %1008
  %1010 = load ptr, ptr %4, align 8, !tbaa !5
  %1011 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1010, i32 0, i32 17
  %1012 = load i32, ptr %5, align 4, !tbaa !9
  %1013 = zext i32 %1012 to i64
  %1014 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1011, i64 noundef %1013) #9
  %1015 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1014, i64 noundef 0, i64 noundef 0)
  store double %1009, ptr %1015, align 8, !tbaa !15
  %1016 = load ptr, ptr %4, align 8, !tbaa !5
  %1017 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1016, i32 0, i32 17
  %1018 = load i32, ptr %5, align 4, !tbaa !9
  %1019 = zext i32 %1018 to i64
  %1020 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1017, i64 noundef %1019) #9
  %1021 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1020, i64 noundef 0, i64 noundef 2)
  store double 1.000000e+00, ptr %1021, align 8, !tbaa !15
  %1022 = load double, ptr %56, align 8, !tbaa !15
  %1023 = load double, ptr %57, align 8, !tbaa !15
  %1024 = fmul double %1022, %1023
  %1025 = load ptr, ptr %4, align 8, !tbaa !5
  %1026 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1025, i32 0, i32 17
  %1027 = load i32, ptr %5, align 4, !tbaa !9
  %1028 = zext i32 %1027 to i64
  %1029 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1026, i64 noundef %1028) #9
  %1030 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1029, i64 noundef 1, i64 noundef 0)
  store double %1024, ptr %1030, align 8, !tbaa !15
  %1031 = load double, ptr %58, align 8, !tbaa !15
  %1032 = load ptr, ptr %4, align 8, !tbaa !5
  %1033 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1032, i32 0, i32 17
  %1034 = load i32, ptr %5, align 4, !tbaa !9
  %1035 = zext i32 %1034 to i64
  %1036 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1033, i64 noundef %1035) #9
  %1037 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1036, i64 noundef 1, i64 noundef 1)
  store double %1031, ptr %1037, align 8, !tbaa !15
  %1038 = load double, ptr %56, align 8, !tbaa !15
  %1039 = load double, ptr %58, align 8, !tbaa !15
  %1040 = fmul double %1038, %1039
  %1041 = load ptr, ptr %4, align 8, !tbaa !5
  %1042 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1041, i32 0, i32 17
  %1043 = load i32, ptr %5, align 4, !tbaa !9
  %1044 = zext i32 %1043 to i64
  %1045 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1042, i64 noundef %1044) #9
  %1046 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1045, i64 noundef 2, i64 noundef 0)
  store double %1040, ptr %1046, align 8, !tbaa !15
  %1047 = load double, ptr %57, align 8, !tbaa !15
  %1048 = fneg double %1047
  %1049 = load ptr, ptr %4, align 8, !tbaa !5
  %1050 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1049, i32 0, i32 17
  %1051 = load i32, ptr %5, align 4, !tbaa !9
  %1052 = zext i32 %1051 to i64
  %1053 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1050, i64 noundef %1052) #9
  %1054 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1053, i64 noundef 2, i64 noundef 1)
  store double %1048, ptr %1054, align 8, !tbaa !15
  call void @llvm.lifetime.end.p0(i64 8, ptr %58) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %57) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %56) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %55) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %54) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %53) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %52) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %51) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %50) #9
  br label %1654

1055:                                             ; preds = %892
  %1056 = load ptr, ptr %4, align 8, !tbaa !5
  %1057 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1056, i32 0, i32 10
  %1058 = load i32, ptr %5, align 4, !tbaa !9
  %1059 = zext i32 %1058 to i64
  %1060 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1057, i64 noundef %1059) #9
  %1061 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1060, i32 0, i32 1
  %1062 = load i32, ptr %1061, align 8, !tbaa !11
  %1063 = icmp eq i32 %1062, 8
  br i1 %1063, label %1064, label %1217

1064:                                             ; preds = %1055
  call void @llvm.lifetime.start.p0(i64 8, ptr %75) #9
  %1065 = load ptr, ptr %6, align 8, !tbaa !5
  %1066 = load ptr, ptr %4, align 8, !tbaa !5
  %1067 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1066, i32 0, i32 10
  %1068 = load i32, ptr %5, align 4, !tbaa !9
  %1069 = zext i32 %1068 to i64
  %1070 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1067, i64 noundef %1069) #9
  %1071 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1070, i32 0, i32 3
  %1072 = load i32, ptr %1071, align 8, !tbaa !14
  %1073 = zext i32 %1072 to i64
  %1074 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1065, i64 noundef %1073)
  %1075 = load double, ptr %1074, align 8, !tbaa !15
  store double %1075, ptr %75, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %76) #9
  %1076 = load ptr, ptr %6, align 8, !tbaa !5
  %1077 = load ptr, ptr %4, align 8, !tbaa !5
  %1078 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1077, i32 0, i32 10
  %1079 = load i32, ptr %5, align 4, !tbaa !9
  %1080 = zext i32 %1079 to i64
  %1081 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1078, i64 noundef %1080) #9
  %1082 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1081, i32 0, i32 3
  %1083 = load i32, ptr %1082, align 8, !tbaa !14
  %1084 = add i32 %1083, 1
  %1085 = zext i32 %1084 to i64
  %1086 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1076, i64 noundef %1085)
  %1087 = load double, ptr %1086, align 8, !tbaa !15
  store double %1087, ptr %76, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %77) #9
  %1088 = load ptr, ptr %6, align 8, !tbaa !5
  %1089 = load ptr, ptr %4, align 8, !tbaa !5
  %1090 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1089, i32 0, i32 10
  %1091 = load i32, ptr %5, align 4, !tbaa !9
  %1092 = zext i32 %1091 to i64
  %1093 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1090, i64 noundef %1092) #9
  %1094 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1093, i32 0, i32 3
  %1095 = load i32, ptr %1094, align 8, !tbaa !14
  %1096 = add i32 %1095, 2
  %1097 = zext i32 %1096 to i64
  %1098 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1088, i64 noundef %1097)
  %1099 = load double, ptr %1098, align 8, !tbaa !15
  store double %1099, ptr %77, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %78) #9
  %1100 = load double, ptr %75, align 8, !tbaa !15
  %1101 = call double @sin(double noundef %1100) #9
  store double %1101, ptr %78, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %79) #9
  %1102 = load double, ptr %75, align 8, !tbaa !15
  %1103 = call double @cos(double noundef %1102) #9
  store double %1103, ptr %79, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %80) #9
  %1104 = load double, ptr %76, align 8, !tbaa !15
  %1105 = call double @sin(double noundef %1104) #9
  store double %1105, ptr %80, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %81) #9
  %1106 = load double, ptr %76, align 8, !tbaa !15
  %1107 = call double @cos(double noundef %1106) #9
  store double %1107, ptr %81, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %82) #9
  %1108 = load double, ptr %77, align 8, !tbaa !15
  %1109 = call double @sin(double noundef %1108) #9
  store double %1109, ptr %82, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %83) #9
  %1110 = load double, ptr %77, align 8, !tbaa !15
  %1111 = call double @cos(double noundef %1110) #9
  store double %1111, ptr %83, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 96, ptr %84) #9
  call void @llvm.lifetime.start.p0(i64 96, ptr %85) #9
  call void @llvm.lifetime.start.p0(i64 72, ptr %86) #9
  call void @llvm.lifetime.start.p0(i64 8, ptr %87) #9
  %1112 = load double, ptr %83, align 8, !tbaa !15
  %1113 = load double, ptr %81, align 8, !tbaa !15
  %1114 = fmul double %1112, %1113
  store double %1114, ptr %87, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %88) #9
  %1115 = load double, ptr %82, align 8, !tbaa !15
  %1116 = load double, ptr %79, align 8, !tbaa !15
  %1117 = load double, ptr %83, align 8, !tbaa !15
  %1118 = load double, ptr %80, align 8, !tbaa !15
  %1119 = fmul double %1117, %1118
  %1120 = load double, ptr %78, align 8, !tbaa !15
  %1121 = fmul double %1119, %1120
  %1122 = call double @llvm.fmuladd.f64(double %1115, double %1116, double %1121)
  store double %1122, ptr %88, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %89) #9
  %1123 = load double, ptr %82, align 8, !tbaa !15
  %1124 = load double, ptr %78, align 8, !tbaa !15
  %1125 = load double, ptr %83, align 8, !tbaa !15
  %1126 = load double, ptr %80, align 8, !tbaa !15
  %1127 = fmul double %1125, %1126
  %1128 = load double, ptr %79, align 8, !tbaa !15
  %1129 = fmul double %1127, %1128
  %1130 = fneg double %1129
  %1131 = call double @llvm.fmuladd.f64(double %1123, double %1124, double %1130)
  store double %1131, ptr %89, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %90) #9
  %1132 = load double, ptr %82, align 8, !tbaa !15
  %1133 = fneg double %1132
  %1134 = load double, ptr %81, align 8, !tbaa !15
  %1135 = fmul double %1133, %1134
  store double %1135, ptr %90, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %91) #9
  %1136 = load double, ptr %83, align 8, !tbaa !15
  %1137 = load double, ptr %79, align 8, !tbaa !15
  %1138 = load double, ptr %82, align 8, !tbaa !15
  %1139 = load double, ptr %80, align 8, !tbaa !15
  %1140 = fmul double %1138, %1139
  %1141 = load double, ptr %78, align 8, !tbaa !15
  %1142 = fmul double %1140, %1141
  %1143 = fneg double %1142
  %1144 = call double @llvm.fmuladd.f64(double %1136, double %1137, double %1143)
  store double %1144, ptr %91, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %92) #9
  %1145 = load double, ptr %83, align 8, !tbaa !15
  %1146 = load double, ptr %78, align 8, !tbaa !15
  %1147 = load double, ptr %82, align 8, !tbaa !15
  %1148 = load double, ptr %80, align 8, !tbaa !15
  %1149 = fmul double %1147, %1148
  %1150 = load double, ptr %79, align 8, !tbaa !15
  %1151 = fmul double %1149, %1150
  %1152 = call double @llvm.fmuladd.f64(double %1145, double %1146, double %1151)
  store double %1152, ptr %92, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %93) #9
  %1153 = load double, ptr %81, align 8, !tbaa !15
  %1154 = fneg double %1153
  %1155 = load double, ptr %78, align 8, !tbaa !15
  %1156 = fmul double %1154, %1155
  store double %1156, ptr %93, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %94) #9
  %1157 = load double, ptr %81, align 8, !tbaa !15
  %1158 = load double, ptr %79, align 8, !tbaa !15
  %1159 = fmul double %1157, %1158
  store double %1159, ptr %94, align 8, !tbaa !15
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %86, ptr noundef nonnull align 8 dereferenceable(8) %87, ptr noundef nonnull align 8 dereferenceable(8) %88, ptr noundef nonnull align 8 dereferenceable(8) %89, ptr noundef nonnull align 8 dereferenceable(8) %90, ptr noundef nonnull align 8 dereferenceable(8) %91, ptr noundef nonnull align 8 dereferenceable(8) %92, ptr noundef nonnull align 8 dereferenceable(8) %80, ptr noundef nonnull align 8 dereferenceable(8) %93, ptr noundef nonnull align 8 dereferenceable(8) %94)
  call void @llvm.lifetime.start.p0(i64 24, ptr %95) #9
  call void @llvm.lifetime.start.p0(i64 8, ptr %96) #9
  store double 0.000000e+00, ptr %96, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %97) #9
  store double 0.000000e+00, ptr %97, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %98) #9
  store double 0.000000e+00, ptr %98, align 8, !tbaa !15
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %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)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %85, ptr noundef nonnull align 8 dereferenceable(72) %86, ptr noundef nonnull align 8 dereferenceable(24) %95)
  %1160 = load ptr, ptr %4, align 8, !tbaa !5
  %1161 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1160, i32 0, i32 15
  %1162 = load i32, ptr %5, align 4, !tbaa !9
  %1163 = zext i32 %1162 to i64
  %1164 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1161, i64 noundef %1163) #9
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %84, ptr noundef nonnull align 8 dereferenceable(96) %85, ptr noundef nonnull align 8 dereferenceable(96) %1164)
  %1165 = load ptr, ptr %4, align 8, !tbaa !5
  %1166 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1165, i32 0, i32 34
  %1167 = load i32, ptr %5, align 4, !tbaa !9
  %1168 = zext i32 %1167 to i64
  %1169 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1166, i64 noundef %1168) #9
  %1170 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %1169, ptr noundef nonnull align 8 dereferenceable(96) %84) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %98) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %97) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %96) #9
  call void @llvm.lifetime.end.p0(i64 24, ptr %95) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %94) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %93) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %92) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %91) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %90) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %89) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %88) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %87) #9
  call void @llvm.lifetime.end.p0(i64 72, ptr %86) #9
  call void @llvm.lifetime.end.p0(i64 96, ptr %85) #9
  call void @llvm.lifetime.end.p0(i64 96, ptr %84) #9
  %1171 = load double, ptr %83, align 8, !tbaa !15
  %1172 = load double, ptr %81, align 8, !tbaa !15
  %1173 = fmul double %1171, %1172
  %1174 = load ptr, ptr %4, align 8, !tbaa !5
  %1175 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1174, i32 0, i32 17
  %1176 = load i32, ptr %5, align 4, !tbaa !9
  %1177 = zext i32 %1176 to i64
  %1178 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1175, i64 noundef %1177) #9
  %1179 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1178, i64 noundef 0, i64 noundef 0)
  store double %1173, ptr %1179, align 8, !tbaa !15
  %1180 = load double, ptr %82, align 8, !tbaa !15
  %1181 = load ptr, ptr %4, align 8, !tbaa !5
  %1182 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1181, i32 0, i32 17
  %1183 = load i32, ptr %5, align 4, !tbaa !9
  %1184 = zext i32 %1183 to i64
  %1185 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1182, i64 noundef %1184) #9
  %1186 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1185, i64 noundef 0, i64 noundef 1)
  store double %1180, ptr %1186, align 8, !tbaa !15
  %1187 = load double, ptr %82, align 8, !tbaa !15
  %1188 = fneg double %1187
  %1189 = load double, ptr %81, align 8, !tbaa !15
  %1190 = fmul double %1188, %1189
  %1191 = load ptr, ptr %4, align 8, !tbaa !5
  %1192 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1191, i32 0, i32 17
  %1193 = load i32, ptr %5, align 4, !tbaa !9
  %1194 = zext i32 %1193 to i64
  %1195 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1192, i64 noundef %1194) #9
  %1196 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1195, i64 noundef 1, i64 noundef 0)
  store double %1190, ptr %1196, align 8, !tbaa !15
  %1197 = load double, ptr %83, align 8, !tbaa !15
  %1198 = load ptr, ptr %4, align 8, !tbaa !5
  %1199 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1198, i32 0, i32 17
  %1200 = load i32, ptr %5, align 4, !tbaa !9
  %1201 = zext i32 %1200 to i64
  %1202 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1199, i64 noundef %1201) #9
  %1203 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1202, i64 noundef 1, i64 noundef 1)
  store double %1197, ptr %1203, align 8, !tbaa !15
  %1204 = load double, ptr %80, align 8, !tbaa !15
  %1205 = load ptr, ptr %4, align 8, !tbaa !5
  %1206 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1205, i32 0, i32 17
  %1207 = load i32, ptr %5, align 4, !tbaa !9
  %1208 = zext i32 %1207 to i64
  %1209 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1206, i64 noundef %1208) #9
  %1210 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1209, i64 noundef 2, i64 noundef 0)
  store double %1204, ptr %1210, align 8, !tbaa !15
  %1211 = load ptr, ptr %4, align 8, !tbaa !5
  %1212 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1211, i32 0, i32 17
  %1213 = load i32, ptr %5, align 4, !tbaa !9
  %1214 = zext i32 %1213 to i64
  %1215 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1212, i64 noundef %1214) #9
  %1216 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1215, i64 noundef 2, i64 noundef 2)
  store double 1.000000e+00, ptr %1216, align 8, !tbaa !15
  call void @llvm.lifetime.end.p0(i64 8, ptr %83) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %82) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %81) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %80) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %79) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %78) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %77) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %76) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %75) #9
  br label %1653

1217:                                             ; preds = %1055
  %1218 = load ptr, ptr %4, align 8, !tbaa !5
  %1219 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1218, i32 0, i32 10
  %1220 = load i32, ptr %5, align 4, !tbaa !9
  %1221 = zext i32 %1220 to i64
  %1222 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1219, i64 noundef %1221) #9
  %1223 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1222, i32 0, i32 1
  %1224 = load i32, ptr %1223, align 8, !tbaa !11
  %1225 = icmp eq i32 %1224, 9
  br i1 %1225, label %1226, label %1380

1226:                                             ; preds = %1217
  call void @llvm.lifetime.start.p0(i64 8, ptr %99) #9
  %1227 = load ptr, ptr %6, align 8, !tbaa !5
  %1228 = load ptr, ptr %4, align 8, !tbaa !5
  %1229 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1228, i32 0, i32 10
  %1230 = load i32, ptr %5, align 4, !tbaa !9
  %1231 = zext i32 %1230 to i64
  %1232 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1229, i64 noundef %1231) #9
  %1233 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1232, i32 0, i32 3
  %1234 = load i32, ptr %1233, align 8, !tbaa !14
  %1235 = zext i32 %1234 to i64
  %1236 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1227, i64 noundef %1235)
  %1237 = load double, ptr %1236, align 8, !tbaa !15
  store double %1237, ptr %99, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %100) #9
  %1238 = load ptr, ptr %6, align 8, !tbaa !5
  %1239 = load ptr, ptr %4, align 8, !tbaa !5
  %1240 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1239, i32 0, i32 10
  %1241 = load i32, ptr %5, align 4, !tbaa !9
  %1242 = zext i32 %1241 to i64
  %1243 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1240, i64 noundef %1242) #9
  %1244 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1243, i32 0, i32 3
  %1245 = load i32, ptr %1244, align 8, !tbaa !14
  %1246 = add i32 %1245, 1
  %1247 = zext i32 %1246 to i64
  %1248 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1238, i64 noundef %1247)
  %1249 = load double, ptr %1248, align 8, !tbaa !15
  store double %1249, ptr %100, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %101) #9
  %1250 = load ptr, ptr %6, align 8, !tbaa !5
  %1251 = load ptr, ptr %4, align 8, !tbaa !5
  %1252 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1251, i32 0, i32 10
  %1253 = load i32, ptr %5, align 4, !tbaa !9
  %1254 = zext i32 %1253 to i64
  %1255 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1252, i64 noundef %1254) #9
  %1256 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1255, i32 0, i32 3
  %1257 = load i32, ptr %1256, align 8, !tbaa !14
  %1258 = add i32 %1257, 2
  %1259 = zext i32 %1258 to i64
  %1260 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1250, i64 noundef %1259)
  %1261 = load double, ptr %1260, align 8, !tbaa !15
  store double %1261, ptr %101, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %102) #9
  %1262 = load double, ptr %99, align 8, !tbaa !15
  %1263 = call double @sin(double noundef %1262) #9
  store double %1263, ptr %102, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %103) #9
  %1264 = load double, ptr %99, align 8, !tbaa !15
  %1265 = call double @cos(double noundef %1264) #9
  store double %1265, ptr %103, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %104) #9
  %1266 = load double, ptr %100, align 8, !tbaa !15
  %1267 = call double @sin(double noundef %1266) #9
  store double %1267, ptr %104, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %105) #9
  %1268 = load double, ptr %100, align 8, !tbaa !15
  %1269 = call double @cos(double noundef %1268) #9
  store double %1269, ptr %105, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %106) #9
  %1270 = load double, ptr %101, align 8, !tbaa !15
  %1271 = call double @sin(double noundef %1270) #9
  store double %1271, ptr %106, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %107) #9
  %1272 = load double, ptr %101, align 8, !tbaa !15
  %1273 = call double @cos(double noundef %1272) #9
  store double %1273, ptr %107, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 96, ptr %108) #9
  call void @llvm.lifetime.start.p0(i64 96, ptr %109) #9
  call void @llvm.lifetime.start.p0(i64 72, ptr %110) #9
  call void @llvm.lifetime.start.p0(i64 8, ptr %111) #9
  %1274 = load double, ptr %107, align 8, !tbaa !15
  %1275 = load double, ptr %103, align 8, !tbaa !15
  %1276 = load double, ptr %106, align 8, !tbaa !15
  %1277 = load double, ptr %104, align 8, !tbaa !15
  %1278 = fmul double %1276, %1277
  %1279 = load double, ptr %102, align 8, !tbaa !15
  %1280 = fmul double %1278, %1279
  %1281 = call double @llvm.fmuladd.f64(double %1274, double %1275, double %1280)
  store double %1281, ptr %111, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %112) #9
  %1282 = load double, ptr %106, align 8, !tbaa !15
  %1283 = load double, ptr %105, align 8, !tbaa !15
  %1284 = fmul double %1282, %1283
  store double %1284, ptr %112, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %113) #9
  %1285 = load double, ptr %107, align 8, !tbaa !15
  %1286 = fneg double %1285
  %1287 = load double, ptr %102, align 8, !tbaa !15
  %1288 = load double, ptr %106, align 8, !tbaa !15
  %1289 = load double, ptr %104, align 8, !tbaa !15
  %1290 = fmul double %1288, %1289
  %1291 = load double, ptr %103, align 8, !tbaa !15
  %1292 = fmul double %1290, %1291
  %1293 = call double @llvm.fmuladd.f64(double %1286, double %1287, double %1292)
  store double %1293, ptr %113, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %114) #9
  %1294 = load double, ptr %106, align 8, !tbaa !15
  %1295 = fneg double %1294
  %1296 = load double, ptr %103, align 8, !tbaa !15
  %1297 = load double, ptr %107, align 8, !tbaa !15
  %1298 = load double, ptr %104, align 8, !tbaa !15
  %1299 = fmul double %1297, %1298
  %1300 = load double, ptr %102, align 8, !tbaa !15
  %1301 = fmul double %1299, %1300
  %1302 = call double @llvm.fmuladd.f64(double %1295, double %1296, double %1301)
  store double %1302, ptr %114, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %115) #9
  %1303 = load double, ptr %107, align 8, !tbaa !15
  %1304 = load double, ptr %105, align 8, !tbaa !15
  %1305 = fmul double %1303, %1304
  store double %1305, ptr %115, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %116) #9
  %1306 = load double, ptr %106, align 8, !tbaa !15
  %1307 = load double, ptr %102, align 8, !tbaa !15
  %1308 = load double, ptr %107, align 8, !tbaa !15
  %1309 = load double, ptr %104, align 8, !tbaa !15
  %1310 = fmul double %1308, %1309
  %1311 = load double, ptr %103, align 8, !tbaa !15
  %1312 = fmul double %1310, %1311
  %1313 = call double @llvm.fmuladd.f64(double %1306, double %1307, double %1312)
  store double %1313, ptr %116, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %117) #9
  %1314 = load double, ptr %105, align 8, !tbaa !15
  %1315 = load double, ptr %102, align 8, !tbaa !15
  %1316 = fmul double %1314, %1315
  store double %1316, ptr %117, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %118) #9
  %1317 = load double, ptr %104, align 8, !tbaa !15
  %1318 = fneg double %1317
  store double %1318, ptr %118, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %119) #9
  %1319 = load double, ptr %105, align 8, !tbaa !15
  %1320 = load double, ptr %103, align 8, !tbaa !15
  %1321 = fmul double %1319, %1320
  store double %1321, ptr %119, align 8, !tbaa !15
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %110, ptr noundef nonnull align 8 dereferenceable(8) %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) %118, ptr noundef nonnull align 8 dereferenceable(8) %119)
  call void @llvm.lifetime.start.p0(i64 24, ptr %120) #9
  call void @llvm.lifetime.start.p0(i64 8, ptr %121) #9
  store double 0.000000e+00, ptr %121, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %122) #9
  store double 0.000000e+00, ptr %122, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %123) #9
  store double 0.000000e+00, ptr %123, align 8, !tbaa !15
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %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 @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %109, ptr noundef nonnull align 8 dereferenceable(72) %110, ptr noundef nonnull align 8 dereferenceable(24) %120)
  %1322 = load ptr, ptr %4, align 8, !tbaa !5
  %1323 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1322, i32 0, i32 15
  %1324 = load i32, ptr %5, align 4, !tbaa !9
  %1325 = zext i32 %1324 to i64
  %1326 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1323, i64 noundef %1325) #9
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %108, ptr noundef nonnull align 8 dereferenceable(96) %109, ptr noundef nonnull align 8 dereferenceable(96) %1326)
  %1327 = load ptr, ptr %4, align 8, !tbaa !5
  %1328 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1327, i32 0, i32 34
  %1329 = load i32, ptr %5, align 4, !tbaa !9
  %1330 = zext i32 %1329 to i64
  %1331 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1328, i64 noundef %1330) #9
  %1332 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %1331, ptr noundef nonnull align 8 dereferenceable(96) %108) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %123) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %122) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %121) #9
  call void @llvm.lifetime.end.p0(i64 24, ptr %120) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %119) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %118) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %117) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %116) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %115) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %114) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %113) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %112) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %111) #9
  call void @llvm.lifetime.end.p0(i64 72, ptr %110) #9
  call void @llvm.lifetime.end.p0(i64 96, ptr %109) #9
  call void @llvm.lifetime.end.p0(i64 96, ptr %108) #9
  %1333 = load double, ptr %106, align 8, !tbaa !15
  %1334 = load double, ptr %105, align 8, !tbaa !15
  %1335 = fmul double %1333, %1334
  %1336 = load ptr, ptr %4, align 8, !tbaa !5
  %1337 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1336, i32 0, i32 17
  %1338 = load i32, ptr %5, align 4, !tbaa !9
  %1339 = zext i32 %1338 to i64
  %1340 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1337, i64 noundef %1339) #9
  %1341 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1340, i64 noundef 0, i64 noundef 0)
  store double %1335, ptr %1341, align 8, !tbaa !15
  %1342 = load double, ptr %107, align 8, !tbaa !15
  %1343 = load ptr, ptr %4, align 8, !tbaa !5
  %1344 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1343, i32 0, i32 17
  %1345 = load i32, ptr %5, align 4, !tbaa !9
  %1346 = zext i32 %1345 to i64
  %1347 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1344, i64 noundef %1346) #9
  %1348 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1347, i64 noundef 0, i64 noundef 1)
  store double %1342, ptr %1348, align 8, !tbaa !15
  %1349 = load double, ptr %107, align 8, !tbaa !15
  %1350 = load double, ptr %105, align 8, !tbaa !15
  %1351 = fmul double %1349, %1350
  %1352 = load ptr, ptr %4, align 8, !tbaa !5
  %1353 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1352, i32 0, i32 17
  %1354 = load i32, ptr %5, align 4, !tbaa !9
  %1355 = zext i32 %1354 to i64
  %1356 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1353, i64 noundef %1355) #9
  %1357 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1356, i64 noundef 1, i64 noundef 0)
  store double %1351, ptr %1357, align 8, !tbaa !15
  %1358 = load double, ptr %106, align 8, !tbaa !15
  %1359 = fneg double %1358
  %1360 = load ptr, ptr %4, align 8, !tbaa !5
  %1361 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1360, i32 0, i32 17
  %1362 = load i32, ptr %5, align 4, !tbaa !9
  %1363 = zext i32 %1362 to i64
  %1364 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1361, i64 noundef %1363) #9
  %1365 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1364, i64 noundef 1, i64 noundef 1)
  store double %1359, ptr %1365, align 8, !tbaa !15
  %1366 = load double, ptr %104, align 8, !tbaa !15
  %1367 = fneg double %1366
  %1368 = load ptr, ptr %4, align 8, !tbaa !5
  %1369 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1368, i32 0, i32 17
  %1370 = load i32, ptr %5, align 4, !tbaa !9
  %1371 = zext i32 %1370 to i64
  %1372 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1369, i64 noundef %1371) #9
  %1373 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1372, i64 noundef 2, i64 noundef 0)
  store double %1367, ptr %1373, align 8, !tbaa !15
  %1374 = load ptr, ptr %4, align 8, !tbaa !5
  %1375 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1374, i32 0, i32 17
  %1376 = load i32, ptr %5, align 4, !tbaa !9
  %1377 = zext i32 %1376 to i64
  %1378 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1375, i64 noundef %1377) #9
  %1379 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1378, i64 noundef 2, i64 noundef 2)
  store double 1.000000e+00, ptr %1379, align 8, !tbaa !15
  call void @llvm.lifetime.end.p0(i64 8, ptr %107) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %106) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %105) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %104) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %103) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %102) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %101) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %100) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %99) #9
  br label %1652

1380:                                             ; preds = %1217
  %1381 = load ptr, ptr %4, align 8, !tbaa !5
  %1382 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1381, i32 0, i32 10
  %1383 = load i32, ptr %5, align 4, !tbaa !9
  %1384 = zext i32 %1383 to i64
  %1385 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1382, i64 noundef %1384) #9
  %1386 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1385, i32 0, i32 1
  %1387 = load i32, ptr %1386, align 8, !tbaa !11
  %1388 = icmp eq i32 %1387, 10
  br i1 %1388, label %1389, label %1542

1389:                                             ; preds = %1380
  call void @llvm.lifetime.start.p0(i64 8, ptr %124) #9
  %1390 = load ptr, ptr %6, align 8, !tbaa !5
  %1391 = load ptr, ptr %4, align 8, !tbaa !5
  %1392 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1391, i32 0, i32 10
  %1393 = load i32, ptr %5, align 4, !tbaa !9
  %1394 = zext i32 %1393 to i64
  %1395 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1392, i64 noundef %1394) #9
  %1396 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1395, i32 0, i32 3
  %1397 = load i32, ptr %1396, align 8, !tbaa !14
  %1398 = zext i32 %1397 to i64
  %1399 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1390, i64 noundef %1398)
  %1400 = load double, ptr %1399, align 8, !tbaa !15
  store double %1400, ptr %124, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %125) #9
  %1401 = load ptr, ptr %6, align 8, !tbaa !5
  %1402 = load ptr, ptr %4, align 8, !tbaa !5
  %1403 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1402, i32 0, i32 10
  %1404 = load i32, ptr %5, align 4, !tbaa !9
  %1405 = zext i32 %1404 to i64
  %1406 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1403, i64 noundef %1405) #9
  %1407 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1406, i32 0, i32 3
  %1408 = load i32, ptr %1407, align 8, !tbaa !14
  %1409 = add i32 %1408, 1
  %1410 = zext i32 %1409 to i64
  %1411 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1401, i64 noundef %1410)
  %1412 = load double, ptr %1411, align 8, !tbaa !15
  store double %1412, ptr %125, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %126) #9
  %1413 = load ptr, ptr %6, align 8, !tbaa !5
  %1414 = load ptr, ptr %4, align 8, !tbaa !5
  %1415 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1414, i32 0, i32 10
  %1416 = load i32, ptr %5, align 4, !tbaa !9
  %1417 = zext i32 %1416 to i64
  %1418 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1415, i64 noundef %1417) #9
  %1419 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1418, i32 0, i32 3
  %1420 = load i32, ptr %1419, align 8, !tbaa !14
  %1421 = add i32 %1420, 2
  %1422 = zext i32 %1421 to i64
  %1423 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1413, i64 noundef %1422)
  %1424 = load double, ptr %1423, align 8, !tbaa !15
  store double %1424, ptr %126, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %127) #9
  %1425 = load double, ptr %124, align 8, !tbaa !15
  %1426 = call double @sin(double noundef %1425) #9
  store double %1426, ptr %127, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %128) #9
  %1427 = load double, ptr %124, align 8, !tbaa !15
  %1428 = call double @cos(double noundef %1427) #9
  store double %1428, ptr %128, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %129) #9
  %1429 = load double, ptr %125, align 8, !tbaa !15
  %1430 = call double @sin(double noundef %1429) #9
  store double %1430, ptr %129, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %130) #9
  %1431 = load double, ptr %125, align 8, !tbaa !15
  %1432 = call double @cos(double noundef %1431) #9
  store double %1432, ptr %130, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %131) #9
  %1433 = load double, ptr %126, align 8, !tbaa !15
  %1434 = call double @sin(double noundef %1433) #9
  store double %1434, ptr %131, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %132) #9
  %1435 = load double, ptr %126, align 8, !tbaa !15
  %1436 = call double @cos(double noundef %1435) #9
  store double %1436, ptr %132, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 96, ptr %133) #9
  call void @llvm.lifetime.start.p0(i64 96, ptr %134) #9
  call void @llvm.lifetime.start.p0(i64 72, ptr %135) #9
  call void @llvm.lifetime.start.p0(i64 8, ptr %136) #9
  %1437 = load double, ptr %127, align 8, !tbaa !15
  %1438 = fneg double %1437
  %1439 = load double, ptr %129, align 8, !tbaa !15
  %1440 = fmul double %1438, %1439
  %1441 = load double, ptr %131, align 8, !tbaa !15
  %1442 = load double, ptr %128, align 8, !tbaa !15
  %1443 = load double, ptr %132, align 8, !tbaa !15
  %1444 = fmul double %1442, %1443
  %1445 = call double @llvm.fmuladd.f64(double %1440, double %1441, double %1444)
  store double %1445, ptr %136, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %137) #9
  %1446 = load double, ptr %127, align 8, !tbaa !15
  %1447 = load double, ptr %132, align 8, !tbaa !15
  %1448 = load double, ptr %129, align 8, !tbaa !15
  %1449 = load double, ptr %131, align 8, !tbaa !15
  %1450 = fmul double %1448, %1449
  %1451 = load double, ptr %128, align 8, !tbaa !15
  %1452 = fmul double %1450, %1451
  %1453 = call double @llvm.fmuladd.f64(double %1446, double %1447, double %1452)
  store double %1453, ptr %137, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %138) #9
  %1454 = load double, ptr %131, align 8, !tbaa !15
  %1455 = fneg double %1454
  %1456 = load double, ptr %130, align 8, !tbaa !15
  %1457 = fmul double %1455, %1456
  store double %1457, ptr %138, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %139) #9
  %1458 = load double, ptr %127, align 8, !tbaa !15
  %1459 = fneg double %1458
  %1460 = load double, ptr %130, align 8, !tbaa !15
  %1461 = fmul double %1459, %1460
  store double %1461, ptr %139, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %140) #9
  %1462 = load double, ptr %128, align 8, !tbaa !15
  %1463 = load double, ptr %130, align 8, !tbaa !15
  %1464 = fmul double %1462, %1463
  store double %1464, ptr %140, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %141) #9
  %1465 = load double, ptr %127, align 8, !tbaa !15
  %1466 = load double, ptr %129, align 8, !tbaa !15
  %1467 = fmul double %1465, %1466
  %1468 = load double, ptr %132, align 8, !tbaa !15
  %1469 = load double, ptr %131, align 8, !tbaa !15
  %1470 = load double, ptr %128, align 8, !tbaa !15
  %1471 = fmul double %1469, %1470
  %1472 = call double @llvm.fmuladd.f64(double %1467, double %1468, double %1471)
  store double %1472, ptr %141, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %142) #9
  %1473 = load double, ptr %127, align 8, !tbaa !15
  %1474 = load double, ptr %131, align 8, !tbaa !15
  %1475 = load double, ptr %129, align 8, !tbaa !15
  %1476 = load double, ptr %128, align 8, !tbaa !15
  %1477 = fmul double %1475, %1476
  %1478 = load double, ptr %132, align 8, !tbaa !15
  %1479 = fmul double %1477, %1478
  %1480 = fneg double %1479
  %1481 = call double @llvm.fmuladd.f64(double %1473, double %1474, double %1480)
  store double %1481, ptr %142, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %143) #9
  %1482 = load double, ptr %130, align 8, !tbaa !15
  %1483 = load double, ptr %132, align 8, !tbaa !15
  %1484 = fmul double %1482, %1483
  store double %1484, ptr %143, align 8, !tbaa !15
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %135, ptr noundef nonnull align 8 dereferenceable(8) %136, ptr noundef nonnull align 8 dereferenceable(8) %137, ptr noundef nonnull align 8 dereferenceable(8) %138, ptr noundef nonnull align 8 dereferenceable(8) %139, ptr noundef nonnull align 8 dereferenceable(8) %140, ptr noundef nonnull align 8 dereferenceable(8) %129, ptr noundef nonnull align 8 dereferenceable(8) %141, ptr noundef nonnull align 8 dereferenceable(8) %142, ptr noundef nonnull align 8 dereferenceable(8) %143)
  call void @llvm.lifetime.start.p0(i64 24, ptr %144) #9
  call void @llvm.lifetime.start.p0(i64 16, ptr %145) #9
  call void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE4ZeroEv(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseNullaryOp.149") align 8 %145)
  call void @_ZN9Vector3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %144, ptr noundef nonnull align 1 dereferenceable(1) %145)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %134, ptr noundef nonnull align 8 dereferenceable(72) %135, ptr noundef nonnull align 8 dereferenceable(24) %144)
  %1485 = load ptr, ptr %4, align 8, !tbaa !5
  %1486 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1485, i32 0, i32 15
  %1487 = load i32, ptr %5, align 4, !tbaa !9
  %1488 = zext i32 %1487 to i64
  %1489 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1486, i64 noundef %1488) #9
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %133, ptr noundef nonnull align 8 dereferenceable(96) %134, ptr noundef nonnull align 8 dereferenceable(96) %1489)
  %1490 = load ptr, ptr %4, align 8, !tbaa !5
  %1491 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1490, i32 0, i32 34
  %1492 = load i32, ptr %5, align 4, !tbaa !9
  %1493 = zext i32 %1492 to i64
  %1494 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1491, i64 noundef %1493) #9
  %1495 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %1494, ptr noundef nonnull align 8 dereferenceable(96) %133) #9
  call void @llvm.lifetime.end.p0(i64 16, ptr %145) #9
  call void @llvm.lifetime.end.p0(i64 24, ptr %144) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %143) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %142) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %141) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %140) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %139) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %138) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %137) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %136) #9
  call void @llvm.lifetime.end.p0(i64 72, ptr %135) #9
  call void @llvm.lifetime.end.p0(i64 96, ptr %134) #9
  call void @llvm.lifetime.end.p0(i64 96, ptr %133) #9
  %1496 = load double, ptr %131, align 8, !tbaa !15
  %1497 = fneg double %1496
  %1498 = load double, ptr %130, align 8, !tbaa !15
  %1499 = fmul double %1497, %1498
  %1500 = load ptr, ptr %4, align 8, !tbaa !5
  %1501 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1500, i32 0, i32 17
  %1502 = load i32, ptr %5, align 4, !tbaa !9
  %1503 = zext i32 %1502 to i64
  %1504 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1501, i64 noundef %1503) #9
  %1505 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1504, i64 noundef 0, i64 noundef 0)
  store double %1499, ptr %1505, align 8, !tbaa !15
  %1506 = load double, ptr %132, align 8, !tbaa !15
  %1507 = load ptr, ptr %4, align 8, !tbaa !5
  %1508 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1507, i32 0, i32 17
  %1509 = load i32, ptr %5, align 4, !tbaa !9
  %1510 = zext i32 %1509 to i64
  %1511 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1508, i64 noundef %1510) #9
  %1512 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1511, i64 noundef 0, i64 noundef 1)
  store double %1506, ptr %1512, align 8, !tbaa !15
  %1513 = load double, ptr %129, align 8, !tbaa !15
  %1514 = load ptr, ptr %4, align 8, !tbaa !5
  %1515 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1514, i32 0, i32 17
  %1516 = load i32, ptr %5, align 4, !tbaa !9
  %1517 = zext i32 %1516 to i64
  %1518 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1515, i64 noundef %1517) #9
  %1519 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1518, i64 noundef 1, i64 noundef 0)
  store double %1513, ptr %1519, align 8, !tbaa !15
  %1520 = load ptr, ptr %4, align 8, !tbaa !5
  %1521 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1520, i32 0, i32 17
  %1522 = load i32, ptr %5, align 4, !tbaa !9
  %1523 = zext i32 %1522 to i64
  %1524 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1521, i64 noundef %1523) #9
  %1525 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1524, i64 noundef 1, i64 noundef 2)
  store double 1.000000e+00, ptr %1525, align 8, !tbaa !15
  %1526 = load double, ptr %130, align 8, !tbaa !15
  %1527 = load double, ptr %132, align 8, !tbaa !15
  %1528 = fmul double %1526, %1527
  %1529 = load ptr, ptr %4, align 8, !tbaa !5
  %1530 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1529, i32 0, i32 17
  %1531 = load i32, ptr %5, align 4, !tbaa !9
  %1532 = zext i32 %1531 to i64
  %1533 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1530, i64 noundef %1532) #9
  %1534 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1533, i64 noundef 2, i64 noundef 0)
  store double %1528, ptr %1534, align 8, !tbaa !15
  %1535 = load double, ptr %131, align 8, !tbaa !15
  %1536 = load ptr, ptr %4, align 8, !tbaa !5
  %1537 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1536, i32 0, i32 17
  %1538 = load i32, ptr %5, align 4, !tbaa !9
  %1539 = zext i32 %1538 to i64
  %1540 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1537, i64 noundef %1539) #9
  %1541 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1540, i64 noundef 2, i64 noundef 1)
  store double %1535, ptr %1541, align 8, !tbaa !15
  call void @llvm.lifetime.end.p0(i64 8, ptr %132) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %131) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %130) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %129) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %128) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %127) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %126) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %125) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %124) #9
  br label %1651

1542:                                             ; preds = %1380
  %1543 = load ptr, ptr %4, align 8, !tbaa !5
  %1544 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1543, i32 0, i32 10
  %1545 = load i32, ptr %5, align 4, !tbaa !9
  %1546 = zext i32 %1545 to i64
  %1547 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1544, i64 noundef %1546) #9
  %1548 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1547, i32 0, i32 1
  %1549 = load i32, ptr %1548, align 8, !tbaa !11
  %1550 = icmp eq i32 %1549, 11
  br i1 %1550, label %1551, label %1616

1551:                                             ; preds = %1542
  call void @llvm.lifetime.start.p0(i64 8, ptr %146) #9
  %1552 = load ptr, ptr %6, align 8, !tbaa !5
  %1553 = load ptr, ptr %4, align 8, !tbaa !5
  %1554 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1553, i32 0, i32 10
  %1555 = load i32, ptr %5, align 4, !tbaa !9
  %1556 = zext i32 %1555 to i64
  %1557 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1554, i64 noundef %1556) #9
  %1558 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1557, i32 0, i32 3
  %1559 = load i32, ptr %1558, align 8, !tbaa !14
  %1560 = zext i32 %1559 to i64
  %1561 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1552, i64 noundef %1560)
  %1562 = load double, ptr %1561, align 8, !tbaa !15
  store double %1562, ptr %146, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %147) #9
  %1563 = load ptr, ptr %6, align 8, !tbaa !5
  %1564 = load ptr, ptr %4, align 8, !tbaa !5
  %1565 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1564, i32 0, i32 10
  %1566 = load i32, ptr %5, align 4, !tbaa !9
  %1567 = zext i32 %1566 to i64
  %1568 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1565, i64 noundef %1567) #9
  %1569 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1568, i32 0, i32 3
  %1570 = load i32, ptr %1569, align 8, !tbaa !14
  %1571 = add i32 %1570, 1
  %1572 = zext i32 %1571 to i64
  %1573 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1563, i64 noundef %1572)
  %1574 = load double, ptr %1573, align 8, !tbaa !15
  store double %1574, ptr %147, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 8, ptr %148) #9
  %1575 = load ptr, ptr %6, align 8, !tbaa !5
  %1576 = load ptr, ptr %4, align 8, !tbaa !5
  %1577 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1576, i32 0, i32 10
  %1578 = load i32, ptr %5, align 4, !tbaa !9
  %1579 = zext i32 %1578 to i64
  %1580 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1577, i64 noundef %1579) #9
  %1581 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1580, i32 0, i32 3
  %1582 = load i32, ptr %1581, align 8, !tbaa !14
  %1583 = add i32 %1582, 2
  %1584 = zext i32 %1583 to i64
  %1585 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %1575, i64 noundef %1584)
  %1586 = load double, ptr %1585, align 8, !tbaa !15
  store double %1586, ptr %148, align 8, !tbaa !15
  call void @llvm.lifetime.start.p0(i64 96, ptr %149) #9
  call void @llvm.lifetime.start.p0(i64 96, ptr %150) #9
  call void @llvm.lifetime.start.p0(i64 72, ptr %151) #9
  call void @llvm.lifetime.start.p0(i64 3, ptr %152) #9
  call void @_ZN5Eigen10MatrixBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE8IdentityEll(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseNullaryOp.190") align 1 %152, i64 noundef 3, i64 noundef 3)
  call void @_ZN9Matrix3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_identity_opIdEENS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(72) %151, ptr noundef nonnull align 1 dereferenceable(1) %152)
  call void @llvm.lifetime.start.p0(i64 24, ptr %153) #9
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %153, ptr noundef nonnull align 8 dereferenceable(8) %146, ptr noundef nonnull align 8 dereferenceable(8) %147, ptr noundef nonnull align 8 dereferenceable(8) %148)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %150, ptr noundef nonnull align 8 dereferenceable(72) %151, ptr noundef nonnull align 8 dereferenceable(24) %153)
  %1587 = load ptr, ptr %4, align 8, !tbaa !5
  %1588 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1587, i32 0, i32 15
  %1589 = load i32, ptr %5, align 4, !tbaa !9
  %1590 = zext i32 %1589 to i64
  %1591 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1588, i64 noundef %1590) #9
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %149, ptr noundef nonnull align 8 dereferenceable(96) %150, ptr noundef nonnull align 8 dereferenceable(96) %1591)
  %1592 = load ptr, ptr %4, align 8, !tbaa !5
  %1593 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1592, i32 0, i32 34
  %1594 = load i32, ptr %5, align 4, !tbaa !9
  %1595 = zext i32 %1594 to i64
  %1596 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1593, i64 noundef %1595) #9
  %1597 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %1596, ptr noundef nonnull align 8 dereferenceable(96) %149) #9
  call void @llvm.lifetime.end.p0(i64 24, ptr %153) #9
  call void @llvm.lifetime.end.p0(i64 3, ptr %152) #9
  call void @llvm.lifetime.end.p0(i64 72, ptr %151) #9
  call void @llvm.lifetime.end.p0(i64 96, ptr %150) #9
  call void @llvm.lifetime.end.p0(i64 96, ptr %149) #9
  %1598 = load ptr, ptr %4, align 8, !tbaa !5
  %1599 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1598, i32 0, i32 17
  %1600 = load i32, ptr %5, align 4, !tbaa !9
  %1601 = zext i32 %1600 to i64
  %1602 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1599, i64 noundef %1601) #9
  %1603 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1602, i64 noundef 3, i64 noundef 0)
  store double 1.000000e+00, ptr %1603, align 8, !tbaa !15
  %1604 = load ptr, ptr %4, align 8, !tbaa !5
  %1605 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1604, i32 0, i32 17
  %1606 = load i32, ptr %5, align 4, !tbaa !9
  %1607 = zext i32 %1606 to i64
  %1608 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1605, i64 noundef %1607) #9
  %1609 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1608, i64 noundef 4, i64 noundef 1)
  store double 1.000000e+00, ptr %1609, align 8, !tbaa !15
  %1610 = load ptr, ptr %4, align 8, !tbaa !5
  %1611 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1610, i32 0, i32 17
  %1612 = load i32, ptr %5, align 4, !tbaa !9
  %1613 = zext i32 %1612 to i64
  %1614 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1611, i64 noundef %1613) #9
  %1615 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %1614, i64 noundef 5, i64 noundef 2)
  store double 1.000000e+00, ptr %1615, align 8, !tbaa !15
  call void @llvm.lifetime.end.p0(i64 8, ptr %148) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %147) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %146) #9
  br label %1650

1616:                                             ; preds = %1542
  %1617 = load ptr, ptr %4, align 8, !tbaa !5
  %1618 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1617, i32 0, i32 10
  %1619 = load i32, ptr %5, align 4, !tbaa !9
  %1620 = zext i32 %1619 to i64
  %1621 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1618, i64 noundef %1620) #9
  %1622 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1621, i32 0, i32 1
  %1623 = load i32, ptr %1622, align 8, !tbaa !11
  %1624 = icmp eq i32 %1623, 21
  br i1 %1624, label %1625, label %1646

1625:                                             ; preds = %1616
  call void @llvm.lifetime.start.p0(i64 8, ptr %154) #9
  %1626 = load ptr, ptr %4, align 8, !tbaa !5
  %1627 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1626, i32 0, i32 10
  %1628 = load i32, ptr %5, align 4, !tbaa !9
  %1629 = zext i32 %1628 to i64
  %1630 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1627, i64 noundef %1629) #9
  store ptr %1630, ptr %154, align 8, !tbaa !5
  call void @llvm.lifetime.start.p0(i64 8, ptr %155) #9
  %1631 = load ptr, ptr %4, align 8, !tbaa !5
  %1632 = getelementptr inbounds %"struct.RigidBodyDynamics::Model", ptr %1631, i32 0, i32 22
  %1633 = load ptr, ptr %154, align 8, !tbaa !5
  %1634 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %1633, i32 0, i32 4
  %1635 = load i32, ptr %1634, align 4, !tbaa !19
  %1636 = sext i32 %1635 to i64
  %1637 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSt6vectorIPN17RigidBodyDynamics11CustomJointESaIS2_EEixEm(ptr noundef nonnull align 8 dereferenceable(24) %1632, i64 noundef %1636) #9
  %1638 = load ptr, ptr %1637, align 8, !tbaa !5
  store ptr %1638, ptr %155, align 8, !tbaa !5
  %1639 = load ptr, ptr %155, align 8, !tbaa !5
  %1640 = load ptr, ptr %4, align 8, !tbaa !5
  %1641 = load i32, ptr %5, align 4, !tbaa !9
  %1642 = load ptr, ptr %6, align 8, !tbaa !5
  %1643 = load ptr, ptr %1639, align 8, !tbaa !20
  %1644 = getelementptr inbounds ptr, ptr %1643, i64 3
  %1645 = load ptr, ptr %1644, align 8
  call void %1645(ptr noundef nonnull align 8 dereferenceable(216) %1639, ptr noundef nonnull align 8 dereferenceable(872) %1640, i32 noundef %1641, ptr noundef nonnull align 8 dereferenceable(16) %1642)
  call void @llvm.lifetime.end.p0(i64 8, ptr %155) #9
  call void @llvm.lifetime.end.p0(i64 8, ptr %154) #9
  br label %1649

1646:                                             ; preds = %1616
  %1647 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cerr, ptr noundef @.str.8)
  %1648 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %1647, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
  call void @abort() #8
  unreachable

1649:                                             ; preds = %1625
  br label %1650

1650:                                             ; preds = %1649, %1551
  br label %1651

1651:                                             ; preds = %1650, %1389
  br label %1652

1652:                                             ; preds = %1651, %1226
  br label %1653

1653:                                             ; preds = %1652, %1064
  br label %1654

1654:                                             ; preds = %1653, %901
  br label %1655

1655:                                             ; preds = %1654, %859
  br label %1656

1656:                                             ; preds = %1655, %821
  br label %1657

1657:                                             ; preds = %1656, %743
  br label %1658

1658:                                             ; preds = %1657, %552
  br label %1659

1659:                                             ; preds = %1658, %361
  br label %1660

1660:                                             ; preds = %1659, %170
  ret void
}

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

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

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

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE4ZeroEv(ptr dead_on_unwind noalias writable sret(%"class.Eigen::CwiseNullaryOp.149") align 8) #4 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 #7 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) #1 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) #4

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

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

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

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen10MatrixBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE8IdentityEll(ptr dead_on_unwind noalias writable sret(%"class.Eigen::CwiseNullaryOp.190") align 1, i64 noundef, i64 noundef) #4 align 2

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

attributes #0 = { noreturn nounwind "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #1 = { mustprogress 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 #2 = { nocallback nofree nosync nounwind willreturn memory(argmem: readwrite) }
attributes #3 = { 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 #4 = { 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 #5 = { nocallback nofree nosync nounwind speculatable willreturn memory(none) }
attributes #6 = { inlinehint mustprogress nounwind uwtable "min-legal-vector-width"="0" "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #7 = { 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 #8 = { noreturn nounwind }
attributes #9 = { 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 }
%"struct.RigidBodyDynamics::Math::SpatialTransform" = type { %class.Matrix3_t, %class.Vector3_t }
%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] }
%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::Product" = type { ptr, %"class.Eigen::Block" }
%"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.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" }
%"struct.Eigen::internal::scalar_constant_op" = type { double }
%"class.Eigen::CwiseNullaryOp.190" = type { [3 x i8] }

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

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

; Function Attrs: mustprogress 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 #1 align 2

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

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

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

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

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

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

; 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 #4 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 #1 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 #4 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 #7

; Function Attrs: mustprogress uwtable
define dso_local 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) local_unnamed_addr #7 {
  %4 = alloca double, align 8
  %5 = alloca double, align 8
  %6 = alloca %class.Matrix3_t, align 8
  %7 = alloca double, 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 %class.Matrix3_t, align 8
  %16 = alloca double, 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 %class.Matrix3_t, align 8
  %25 = alloca double, 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 %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %32 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %33 = alloca %class.Vector3_t, align 8
  %34 = alloca %"class.Eigen::Product", align 8
  %35 = alloca %"class.Eigen::Block", align 8
  %36 = alloca %class.SpatialVector_t, align 16
  %37 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %38 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %39 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %40 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %41 = alloca %class.Matrix3_t, align 8
  %42 = alloca %"class.RigidBodyDynamics::Math::Quaternion", align 16
  %43 = alloca %class.Vector3_t, align 8
  %44 = alloca double, align 8
  %45 = alloca double, align 8
  %46 = alloca double, align 8
  %47 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %48 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %49 = alloca %class.Matrix3_t, align 8
  %50 = alloca double, align 8
  %51 = alloca double, align 8
  %52 = alloca double, align 8
  %53 = alloca double, align 8
  %54 = alloca double, align 8
  %55 = alloca double, align 8
  %56 = alloca double, align 8
  %57 = alloca double, align 8
  %58 = alloca double, align 8
  %59 = alloca %class.Vector3_t, align 8
  %60 = alloca double, align 8
  %61 = alloca double, align 8
  %62 = alloca double, align 8
  %63 = alloca double, align 8
  %64 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %65 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %66 = alloca %class.Matrix3_t, 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 %class.Vector3_t, align 8
  %76 = alloca double, align 8
  %77 = alloca double, align 8
  %78 = alloca double, align 8
  %79 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %80 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %81 = alloca %class.Matrix3_t, 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 double, align 8
  %88 = alloca double, align 8
  %89 = alloca double, align 8
  %90 = alloca double, align 8
  %91 = alloca %class.Vector3_t, align 8
  %92 = alloca double, align 8
  %93 = alloca double, align 8
  %94 = alloca double, align 8
  %95 = alloca double, align 8
  %96 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %97 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %98 = alloca %class.Matrix3_t, align 8
  %99 = alloca double, align 8
  %100 = alloca double, 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 %class.Vector3_t, align 8
  %108 = alloca %"class.Eigen::CwiseNullaryOp.149", 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 %"class.Eigen::CwiseNullaryOp.190", align 1
  %116 = alloca %class.Vector3_t, align 8
  %.not = icmp eq i32 %1, 0
  br i1 %.not, label %117, label %118

117:                                              ; preds = %3
  tail call void @__assert_fail(ptr noundef nonnull @.str, ptr noundef nonnull @.str.4, i32 noundef 385, ptr noundef nonnull @__PRETTY_FUNCTION__._ZN17RigidBodyDynamics16jcalc_X_lambda_SERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE) #9
  unreachable

118:                                              ; preds = %3
  %119 = getelementptr inbounds i8, ptr %0, i64 160
  %120 = zext i32 %1 to i64
  %121 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %122 = getelementptr inbounds i8, ptr %121, i64 8
  %123 = load i32, ptr %122, align 8, !tbaa !5
  %124 = icmp eq i32 %123, 3
  br i1 %124, label %125, label %213

125:                                              ; preds = %118
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %4) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %5) #10
  %126 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %127 = getelementptr inbounds i8, ptr %126, i64 16
  %128 = load i32, ptr %127, align 8, !tbaa !12
  %129 = zext i32 %128 to i64
  %130 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %129)
  %131 = load double, ptr %130, align 8, !tbaa !13
  call void @sincos(double noundef %131, ptr noundef nonnull %4, ptr noundef nonnull %5) #10
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %6) #10
  %132 = getelementptr inbounds i8, ptr %0, i64 280
  %133 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %132, i64 noundef %120) #10
  %134 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %133, i64 noundef 0, i64 noundef 0)
  %135 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %132, i64 noundef %120) #10
  %136 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %135, i64 noundef 0, i64 noundef 1)
  %137 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %132, i64 noundef %120) #10
  %138 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %137, i64 noundef 0, i64 noundef 2)
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %7) #10
  %139 = load double, ptr %5, align 8, !tbaa !13
  %140 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %132, i64 noundef %120) #10
  %141 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %140, i64 noundef 1, i64 noundef 0)
  %142 = load double, ptr %141, align 8, !tbaa !13
  %143 = load double, ptr %4, align 8, !tbaa !13
  %144 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %132, i64 noundef %120) #10
  %145 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %144, i64 noundef 2, i64 noundef 0)
  %146 = load double, ptr %145, align 8, !tbaa !13
  %147 = fmul double %143, %146
  %148 = call double @llvm.fmuladd.f64(double %139, double %142, double %147)
  store double %148, ptr %7, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %8) #10
  %149 = load double, ptr %5, align 8, !tbaa !13
  %150 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %132, i64 noundef %120) #10
  %151 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %150, i64 noundef 1, i64 noundef 1)
  %152 = load double, ptr %151, align 8, !tbaa !13
  %153 = load double, ptr %4, align 8, !tbaa !13
  %154 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %132, i64 noundef %120) #10
  %155 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %154, i64 noundef 2, i64 noundef 1)
  %156 = load double, ptr %155, align 8, !tbaa !13
  %157 = fmul double %153, %156
  %158 = call double @llvm.fmuladd.f64(double %149, double %152, double %157)
  store double %158, ptr %8, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %9) #10
  %159 = load double, ptr %5, align 8, !tbaa !13
  %160 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %132, i64 noundef %120) #10
  %161 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %160, i64 noundef 1, i64 noundef 2)
  %162 = load double, ptr %161, align 8, !tbaa !13
  %163 = load double, ptr %4, align 8, !tbaa !13
  %164 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %132, i64 noundef %120) #10
  %165 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %164, i64 noundef 2, i64 noundef 2)
  %166 = load double, ptr %165, align 8, !tbaa !13
  %167 = fmul double %163, %166
  %168 = call double @llvm.fmuladd.f64(double %159, double %162, double %167)
  store double %168, ptr %9, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %10) #10
  %169 = load double, ptr %4, align 8, !tbaa !13
  %170 = fneg double %169
  %171 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %132, i64 noundef %120) #10
  %172 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %171, i64 noundef 1, i64 noundef 0)
  %173 = load double, ptr %172, align 8, !tbaa !13
  %174 = load double, ptr %5, align 8, !tbaa !13
  %175 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %132, i64 noundef %120) #10
  %176 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %175, i64 noundef 2, i64 noundef 0)
  %177 = load double, ptr %176, align 8, !tbaa !13
  %178 = fmul double %174, %177
  %179 = call double @llvm.fmuladd.f64(double %170, double %173, double %178)
  store double %179, ptr %10, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %11) #10
  %180 = load double, ptr %4, align 8, !tbaa !13
  %181 = fneg double %180
  %182 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %132, i64 noundef %120) #10
  %183 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %182, i64 noundef 1, i64 noundef 1)
  %184 = load double, ptr %183, align 8, !tbaa !13
  %185 = load double, ptr %5, align 8, !tbaa !13
  %186 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %132, i64 noundef %120) #10
  %187 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %186, i64 noundef 2, i64 noundef 1)
  %188 = load double, ptr %187, align 8, !tbaa !13
  %189 = fmul double %185, %188
  %190 = call double @llvm.fmuladd.f64(double %181, double %184, double %189)
  store double %190, ptr %11, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %12) #10
  %191 = load double, ptr %4, align 8, !tbaa !13
  %192 = fneg double %191
  %193 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %132, i64 noundef %120) #10
  %194 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %193, i64 noundef 1, i64 noundef 2)
  %195 = load double, ptr %194, align 8, !tbaa !13
  %196 = load double, ptr %5, align 8, !tbaa !13
  %197 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %132, i64 noundef %120) #10
  %198 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %197, i64 noundef 2, i64 noundef 2)
  %199 = load double, ptr %198, align 8, !tbaa !13
  %200 = fmul double %196, %199
  %201 = call double @llvm.fmuladd.f64(double %192, double %195, double %200)
  store double %201, ptr %12, align 8, !tbaa !13
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %6, ptr noundef nonnull align 8 dereferenceable(8) %134, ptr noundef nonnull align 8 dereferenceable(8) %136, ptr noundef nonnull align 8 dereferenceable(8) %138, ptr noundef nonnull align 8 dereferenceable(8) %7, 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)
  %202 = getelementptr inbounds i8, ptr %0, i64 720
  %203 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %202, i64 noundef %120) #10
  %204 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(72) %203, ptr noundef nonnull align 8 dereferenceable(72) %6) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %12) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %11) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %10) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %9) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %8) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %7) #10
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %6) #10
  %205 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %132, i64 noundef %120) #10
  %206 = getelementptr inbounds i8, ptr %205, i64 72
  %207 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %202, i64 noundef %120) #10
  %208 = getelementptr inbounds i8, ptr %207, i64 72
  %209 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSERKS_(ptr noundef nonnull align 8 dereferenceable(24) %208, ptr noundef nonnull align 8 dereferenceable(24) %206)
  %210 = getelementptr inbounds i8, ptr %0, i64 184
  %211 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %210, i64 noundef %120) #10
  %212 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %211, i64 noundef 0)
  store double 1.000000e+00, ptr %212, align 8, !tbaa !13
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %5) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %4) #10
  br label %793

213:                                              ; preds = %118
  %214 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %215 = getelementptr inbounds i8, ptr %214, i64 8
  %216 = load i32, ptr %215, align 8, !tbaa !5
  %217 = icmp eq i32 %216, 4
  br i1 %217, label %218, label %306

218:                                              ; preds = %213
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %13) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %14) #10
  %219 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %220 = getelementptr inbounds i8, ptr %219, i64 16
  %221 = load i32, ptr %220, align 8, !tbaa !12
  %222 = zext i32 %221 to i64
  %223 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %222)
  %224 = load double, ptr %223, align 8, !tbaa !13
  call void @sincos(double noundef %224, ptr noundef nonnull %13, ptr noundef nonnull %14) #10
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %15) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %16) #10
  %225 = load double, ptr %14, align 8, !tbaa !13
  %226 = getelementptr inbounds i8, ptr %0, i64 280
  %227 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %226, i64 noundef %120) #10
  %228 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %227, i64 noundef 0, i64 noundef 0)
  %229 = load double, ptr %228, align 8, !tbaa !13
  %230 = load double, ptr %13, align 8, !tbaa !13
  %231 = fneg double %230
  %232 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %226, i64 noundef %120) #10
  %233 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %232, i64 noundef 2, i64 noundef 0)
  %234 = load double, ptr %233, align 8, !tbaa !13
  %235 = fmul double %234, %231
  %236 = call double @llvm.fmuladd.f64(double %225, double %229, double %235)
  store double %236, ptr %16, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %17) #10
  %237 = load double, ptr %14, align 8, !tbaa !13
  %238 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %226, i64 noundef %120) #10
  %239 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %238, i64 noundef 0, i64 noundef 1)
  %240 = load double, ptr %239, align 8, !tbaa !13
  %241 = load double, ptr %13, align 8, !tbaa !13
  %242 = fneg double %241
  %243 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %226, i64 noundef %120) #10
  %244 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %243, i64 noundef 2, i64 noundef 1)
  %245 = load double, ptr %244, align 8, !tbaa !13
  %246 = fmul double %245, %242
  %247 = call double @llvm.fmuladd.f64(double %237, double %240, double %246)
  store double %247, ptr %17, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %18) #10
  %248 = load double, ptr %14, align 8, !tbaa !13
  %249 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %226, i64 noundef %120) #10
  %250 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %249, i64 noundef 0, i64 noundef 2)
  %251 = load double, ptr %250, align 8, !tbaa !13
  %252 = load double, ptr %13, align 8, !tbaa !13
  %253 = fneg double %252
  %254 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %226, i64 noundef %120) #10
  %255 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %254, i64 noundef 2, i64 noundef 2)
  %256 = load double, ptr %255, align 8, !tbaa !13
  %257 = fmul double %256, %253
  %258 = call double @llvm.fmuladd.f64(double %248, double %251, double %257)
  store double %258, ptr %18, align 8, !tbaa !13
  %259 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %226, i64 noundef %120) #10
  %260 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %259, i64 noundef 1, i64 noundef 0)
  %261 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %226, i64 noundef %120) #10
  %262 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %261, i64 noundef 1, i64 noundef 1)
  %263 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %226, i64 noundef %120) #10
  %264 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %263, i64 noundef 1, i64 noundef 2)
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %19) #10
  %265 = load double, ptr %13, align 8, !tbaa !13
  %266 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %226, i64 noundef %120) #10
  %267 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %266, i64 noundef 0, i64 noundef 0)
  %268 = load double, ptr %267, align 8, !tbaa !13
  %269 = load double, ptr %14, align 8, !tbaa !13
  %270 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %226, i64 noundef %120) #10
  %271 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %270, i64 noundef 2, i64 noundef 0)
  %272 = load double, ptr %271, align 8, !tbaa !13
  %273 = fmul double %269, %272
  %274 = call double @llvm.fmuladd.f64(double %265, double %268, double %273)
  store double %274, ptr %19, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %20) #10
  %275 = load double, ptr %13, align 8, !tbaa !13
  %276 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %226, i64 noundef %120) #10
  %277 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %276, i64 noundef 0, i64 noundef 1)
  %278 = load double, ptr %277, align 8, !tbaa !13
  %279 = load double, ptr %14, align 8, !tbaa !13
  %280 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %226, i64 noundef %120) #10
  %281 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %280, i64 noundef 2, i64 noundef 1)
  %282 = load double, ptr %281, align 8, !tbaa !13
  %283 = fmul double %279, %282
  %284 = call double @llvm.fmuladd.f64(double %275, double %278, double %283)
  store double %284, ptr %20, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %21) #10
  %285 = load double, ptr %13, align 8, !tbaa !13
  %286 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %226, i64 noundef %120) #10
  %287 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %286, i64 noundef 0, i64 noundef 2)
  %288 = load double, ptr %287, align 8, !tbaa !13
  %289 = load double, ptr %14, align 8, !tbaa !13
  %290 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %226, i64 noundef %120) #10
  %291 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %290, i64 noundef 2, i64 noundef 2)
  %292 = load double, ptr %291, align 8, !tbaa !13
  %293 = fmul double %289, %292
  %294 = call double @llvm.fmuladd.f64(double %285, double %288, double %293)
  store double %294, ptr %21, align 8, !tbaa !13
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %15, ptr noundef nonnull align 8 dereferenceable(8) %16, ptr noundef nonnull align 8 dereferenceable(8) %17, ptr noundef nonnull align 8 dereferenceable(8) %18, ptr noundef nonnull align 8 dereferenceable(8) %260, ptr noundef nonnull align 8 dereferenceable(8) %262, ptr noundef nonnull align 8 dereferenceable(8) %264, ptr noundef nonnull align 8 dereferenceable(8) %19, ptr noundef nonnull align 8 dereferenceable(8) %20, ptr noundef nonnull align 8 dereferenceable(8) %21)
  %295 = getelementptr inbounds i8, ptr %0, i64 720
  %296 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %295, i64 noundef %120) #10
  %297 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(72) %296, ptr noundef nonnull align 8 dereferenceable(72) %15) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %21) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %20) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %19) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %18) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %17) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %16) #10
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %15) #10
  %298 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %226, i64 noundef %120) #10
  %299 = getelementptr inbounds i8, ptr %298, i64 72
  %300 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %295, i64 noundef %120) #10
  %301 = getelementptr inbounds i8, ptr %300, i64 72
  %302 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSERKS_(ptr noundef nonnull align 8 dereferenceable(24) %301, ptr noundef nonnull align 8 dereferenceable(24) %299)
  %303 = getelementptr inbounds i8, ptr %0, i64 184
  %304 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %303, i64 noundef %120) #10
  %305 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %304, i64 noundef 1)
  store double 1.000000e+00, ptr %305, align 8, !tbaa !13
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %14) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %13) #10
  br label %793

306:                                              ; preds = %213
  %307 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %308 = getelementptr inbounds i8, ptr %307, i64 8
  %309 = load i32, ptr %308, align 8, !tbaa !5
  %310 = icmp eq i32 %309, 5
  br i1 %310, label %311, label %399

311:                                              ; preds = %306
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %22) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %23) #10
  %312 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %313 = getelementptr inbounds i8, ptr %312, i64 16
  %314 = load i32, ptr %313, align 8, !tbaa !12
  %315 = zext i32 %314 to i64
  %316 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %315)
  %317 = load double, ptr %316, align 8, !tbaa !13
  call void @sincos(double noundef %317, ptr noundef nonnull %22, ptr noundef nonnull %23) #10
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %24) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %25) #10
  %318 = load double, ptr %23, align 8, !tbaa !13
  %319 = getelementptr inbounds i8, ptr %0, i64 280
  %320 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %319, i64 noundef %120) #10
  %321 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %320, i64 noundef 0, i64 noundef 0)
  %322 = load double, ptr %321, align 8, !tbaa !13
  %323 = load double, ptr %22, align 8, !tbaa !13
  %324 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %319, i64 noundef %120) #10
  %325 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %324, i64 noundef 1, i64 noundef 0)
  %326 = load double, ptr %325, align 8, !tbaa !13
  %327 = fmul double %323, %326
  %328 = call double @llvm.fmuladd.f64(double %318, double %322, double %327)
  store double %328, ptr %25, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %26) #10
  %329 = load double, ptr %23, align 8, !tbaa !13
  %330 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %319, i64 noundef %120) #10
  %331 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %330, i64 noundef 0, i64 noundef 1)
  %332 = load double, ptr %331, align 8, !tbaa !13
  %333 = load double, ptr %22, align 8, !tbaa !13
  %334 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %319, i64 noundef %120) #10
  %335 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %334, i64 noundef 1, i64 noundef 1)
  %336 = load double, ptr %335, align 8, !tbaa !13
  %337 = fmul double %333, %336
  %338 = call double @llvm.fmuladd.f64(double %329, double %332, double %337)
  store double %338, ptr %26, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %27) #10
  %339 = load double, ptr %23, align 8, !tbaa !13
  %340 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %319, i64 noundef %120) #10
  %341 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %340, i64 noundef 0, i64 noundef 2)
  %342 = load double, ptr %341, align 8, !tbaa !13
  %343 = load double, ptr %22, align 8, !tbaa !13
  %344 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %319, i64 noundef %120) #10
  %345 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %344, i64 noundef 1, i64 noundef 2)
  %346 = load double, ptr %345, align 8, !tbaa !13
  %347 = fmul double %343, %346
  %348 = call double @llvm.fmuladd.f64(double %339, double %342, double %347)
  store double %348, ptr %27, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %28) #10
  %349 = load double, ptr %22, align 8, !tbaa !13
  %350 = fneg double %349
  %351 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %319, i64 noundef %120) #10
  %352 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %351, i64 noundef 0, i64 noundef 0)
  %353 = load double, ptr %352, align 8, !tbaa !13
  %354 = load double, ptr %23, align 8, !tbaa !13
  %355 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %319, i64 noundef %120) #10
  %356 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %355, i64 noundef 1, i64 noundef 0)
  %357 = load double, ptr %356, align 8, !tbaa !13
  %358 = fmul double %354, %357
  %359 = call double @llvm.fmuladd.f64(double %350, double %353, double %358)
  store double %359, ptr %28, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %29) #10
  %360 = load double, ptr %22, align 8, !tbaa !13
  %361 = fneg double %360
  %362 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %319, i64 noundef %120) #10
  %363 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %362, i64 noundef 0, i64 noundef 1)
  %364 = load double, ptr %363, align 8, !tbaa !13
  %365 = load double, ptr %23, align 8, !tbaa !13
  %366 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %319, i64 noundef %120) #10
  %367 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %366, i64 noundef 1, i64 noundef 1)
  %368 = load double, ptr %367, align 8, !tbaa !13
  %369 = fmul double %365, %368
  %370 = call double @llvm.fmuladd.f64(double %361, double %364, double %369)
  store double %370, ptr %29, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %30) #10
  %371 = load double, ptr %22, align 8, !tbaa !13
  %372 = fneg double %371
  %373 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %319, i64 noundef %120) #10
  %374 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %373, i64 noundef 0, i64 noundef 2)
  %375 = load double, ptr %374, align 8, !tbaa !13
  %376 = load double, ptr %23, align 8, !tbaa !13
  %377 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %319, i64 noundef %120) #10
  %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 !13
  %380 = fmul double %376, %379
  %381 = call double @llvm.fmuladd.f64(double %372, double %375, double %380)
  store double %381, ptr %30, align 8, !tbaa !13
  %382 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %319, i64 noundef %120) #10
  %383 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %382, i64 noundef 2, i64 noundef 0)
  %384 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %319, i64 noundef %120) #10
  %385 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %384, i64 noundef 2, i64 noundef 1)
  %386 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %319, i64 noundef %120) #10
  %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)
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %24, ptr noundef nonnull align 8 dereferenceable(8) %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) %383, ptr noundef nonnull align 8 dereferenceable(8) %385, ptr noundef nonnull align 8 dereferenceable(8) %387)
  %388 = getelementptr inbounds i8, ptr %0, i64 720
  %389 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %388, i64 noundef %120) #10
  %390 = call noundef nonnull align 8 dereferenceable(72) ptr @_ZN9Matrix3_taSEOS_(ptr noundef nonnull align 8 dereferenceable(72) %389, ptr noundef nonnull align 8 dereferenceable(72) %24) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %30) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %29) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %28) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %27) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %26) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %25) #10
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %24) #10
  %391 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %319, i64 noundef %120) #10
  %392 = getelementptr inbounds i8, ptr %391, i64 72
  %393 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %388, i64 noundef %120) #10
  %394 = getelementptr inbounds i8, ptr %393, i64 72
  %395 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZN9Vector3_taSERKS_(ptr noundef nonnull align 8 dereferenceable(24) %394, ptr noundef nonnull align 8 dereferenceable(24) %392)
  %396 = getelementptr inbounds i8, ptr %0, i64 184
  %397 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %396, i64 noundef %120) #10
  %398 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %397, i64 noundef 2)
  store double 1.000000e+00, ptr %398, align 8, !tbaa !13
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %23) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %22) #10
  br label %793

399:                                              ; preds = %306
  %400 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %401 = getelementptr inbounds i8, ptr %400, i64 8
  %402 = load i32, ptr %401, align 8, !tbaa !5
  %403 = icmp eq i32 %402, 14
  br i1 %403, label %404, label %427

404:                                              ; preds = %399
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %31) #10
  call void @_ZN17RigidBodyDynamics8jcalc_XJERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %31, 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 96, ptr nonnull %32) #10
  %405 = getelementptr inbounds i8, ptr %0, i64 280
  %406 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %405, i64 noundef %120) #10
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %32, ptr noundef nonnull align 8 dereferenceable(96) %31, ptr noundef nonnull align 8 dereferenceable(96) %406)
  %407 = getelementptr inbounds i8, ptr %0, i64 720
  %408 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %407, i64 noundef %120) #10
  %409 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %408, ptr noundef nonnull align 8 dereferenceable(96) %32) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %32) #10
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %33) #10
  call void @llvm.lifetime.start.p0(i64 64, ptr nonnull %34) #10
  call void @llvm.lifetime.start.p0(i64 56, ptr nonnull %35) #10
  %410 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %411 = load ptr, ptr %410, 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 %35, ptr noundef nonnull align 1 dereferenceable(1) %411, 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 %34, ptr noundef nonnull align 1 dereferenceable(1) %31, ptr noundef nonnull align 1 dereferenceable(1) %35)
  call void @_ZN9Vector3_tC2IN5Eigen7ProductINS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEENS1_5BlockINS3_IdLi6ELi1ELi0ELi6ELi1EEELin1ELin1ELb0EEELi0EEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %33, ptr noundef nonnull align 1 dereferenceable(1) %34)
  call void @llvm.lifetime.end.p0(i64 56, ptr nonnull %35) #10
  call void @llvm.lifetime.end.p0(i64 64, ptr nonnull %34) #10
  call void @llvm.lifetime.start.p0(i64 48, ptr nonnull %36) #10
  %412 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %413 = load ptr, ptr %412, align 8, !tbaa !15
  %414 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %413, i64 noundef 0)
  %415 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %416 = load ptr, ptr %415, align 8, !tbaa !15
  %417 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %416, i64 noundef 1)
  %418 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %419 = load ptr, ptr %418, align 8, !tbaa !15
  %420 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %419, i64 noundef 2)
  %421 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %33, i64 noundef 0)
  %422 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %33, i64 noundef 1)
  %423 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %33, i64 noundef 2)
  call void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %36, ptr noundef nonnull align 8 dereferenceable(8) %414, ptr noundef nonnull align 8 dereferenceable(8) %417, ptr noundef nonnull align 8 dereferenceable(8) %420, ptr noundef nonnull align 8 dereferenceable(8) %421, ptr noundef nonnull align 8 dereferenceable(8) %422, ptr noundef nonnull align 8 dereferenceable(8) %423)
  %424 = getelementptr inbounds i8, ptr %0, i64 184
  %425 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %424, i64 noundef %120) #10
  %426 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSEOS_(ptr noundef nonnull align 16 dereferenceable(48) %425, ptr noundef nonnull align 16 dereferenceable(48) %36) #10
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %36) #10
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %33) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %31) #10
  br label %793

427:                                              ; preds = %399
  %428 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %429 = getelementptr inbounds i8, ptr %428, i64 12
  %430 = load i32, ptr %429, align 4, !tbaa !16
  %431 = icmp eq i32 %430, 1
  br i1 %431, label %432, label %447

432:                                              ; preds = %427
  %433 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %434 = getelementptr inbounds i8, ptr %433, i64 8
  %435 = load i32, ptr %434, align 8, !tbaa !5
  %.not473 = icmp eq i32 %435, 21
  br i1 %.not473, label %447, label %436

436:                                              ; preds = %432
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %37) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %38) #10
  call void @_ZN17RigidBodyDynamics8jcalc_XJERNS_5ModelEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %38, ptr noundef nonnull align 8 dereferenceable(872) %0, i32 noundef %1, ptr noundef nonnull align 8 dereferenceable(16) %2)
  %437 = getelementptr inbounds i8, ptr %0, i64 280
  %438 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %437, i64 noundef %120) #10
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %37, ptr noundef nonnull align 8 dereferenceable(96) %38, ptr noundef nonnull align 8 dereferenceable(96) %438)
  %439 = getelementptr inbounds i8, ptr %0, i64 720
  %440 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %439, i64 noundef %120) #10
  %441 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %440, ptr noundef nonnull align 8 dereferenceable(96) %37) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %38) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %37) #10
  %442 = call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %443 = load ptr, ptr %442, align 8, !tbaa !15
  %444 = getelementptr inbounds i8, ptr %0, i64 184
  %445 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZNSt6vectorI15SpatialVector_tN5Eigen17aligned_allocatorIS0_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %444, i64 noundef %120) #10
  %446 = call noundef nonnull align 16 dereferenceable(48) ptr @_ZN15SpatialVector_taSERKS_(ptr noundef nonnull align 16 dereferenceable(48) %445, ptr noundef nonnull align 16 dereferenceable(48) %443)
  br label %793

447:                                              ; preds = %432, %427
  %448 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %449 = getelementptr inbounds i8, ptr %448, i64 8
  %450 = load i32, ptr %449, align 8, !tbaa !5
  %451 = icmp eq i32 %450, 6
  br i1 %451, label %452, label %465

452:                                              ; preds = %447
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %39) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %40) #10
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %41) #10
  call void @llvm.lifetime.start.p0(i64 32, ptr nonnull %42) #10
  call void @_ZNK17RigidBodyDynamics5Model13GetQuaternionEjRKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEE(ptr dead_on_unwind nonnull writable sret(%"class.RigidBodyDynamics::Math::Quaternion") align 16 %42, 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 %41, ptr noundef nonnull align 16 dereferenceable(32) %42)
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %43) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %44) #10
  store double 0.000000e+00, ptr %44, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %45) #10
  store double 0.000000e+00, ptr %45, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %46) #10
  store double 0.000000e+00, ptr %46, align 8, !tbaa !13
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %43, ptr noundef nonnull align 8 dereferenceable(8) %44, ptr noundef nonnull align 8 dereferenceable(8) %45, ptr noundef nonnull align 8 dereferenceable(8) %46)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %40, ptr noundef nonnull align 8 dereferenceable(72) %41, ptr noundef nonnull align 8 dereferenceable(24) %43)
  %453 = getelementptr inbounds i8, ptr %0, i64 280
  %454 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %453, i64 noundef %120) #10
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %39, ptr noundef nonnull align 8 dereferenceable(96) %40, ptr noundef nonnull align 8 dereferenceable(96) %454)
  %455 = getelementptr inbounds i8, ptr %0, i64 720
  %456 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %455, i64 noundef %120) #10
  %457 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %456, ptr noundef nonnull align 8 dereferenceable(96) %39) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %46) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %45) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %44) #10
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %43) #10
  call void @llvm.lifetime.end.p0(i64 32, ptr nonnull %42) #10
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %41) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %40) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %39) #10
  %458 = getelementptr inbounds i8, ptr %0, i64 328
  %459 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %458, i64 noundef %120) #10
  %460 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %459, i64 noundef 0, i64 noundef 0)
  store double 1.000000e+00, ptr %460, align 8, !tbaa !13
  %461 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %458, i64 noundef %120) #10
  %462 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %461, i64 noundef 1, i64 noundef 1)
  store double 1.000000e+00, ptr %462, align 8, !tbaa !13
  %463 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %458, i64 noundef %120) #10
  %464 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %463, i64 noundef 2, i64 noundef 2)
  store double 1.000000e+00, ptr %464, align 8, !tbaa !13
  br label %793

465:                                              ; preds = %447
  %466 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %467 = getelementptr inbounds i8, ptr %466, i64 8
  %468 = load i32, ptr %467, align 8, !tbaa !5
  %469 = icmp eq i32 %468, 7
  %470 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  br i1 %469, label %471, label %533

471:                                              ; preds = %465
  %472 = getelementptr inbounds i8, ptr %470, i64 16
  %473 = load i32, ptr %472, align 8, !tbaa !12
  %474 = zext i32 %473 to i64
  %475 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %474)
  %476 = load double, ptr %475, align 8, !tbaa !13
  %477 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %478 = getelementptr inbounds i8, ptr %477, i64 16
  %479 = load i32, ptr %478, align 8, !tbaa !12
  %480 = add i32 %479, 1
  %481 = zext i32 %480 to i64
  %482 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %481)
  %483 = load double, ptr %482, align 8, !tbaa !13
  %484 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %485 = getelementptr inbounds i8, ptr %484, i64 16
  %486 = load i32, ptr %485, align 8, !tbaa !12
  %487 = add i32 %486, 2
  %488 = zext i32 %487 to i64
  %489 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %488)
  %490 = load double, ptr %489, align 8, !tbaa !13
  %491 = tail call double @sin(double noundef %476) #10
  %492 = tail call double @cos(double noundef %476) #10
  %493 = tail call double @sin(double noundef %483) #10
  %494 = tail call double @cos(double noundef %483) #10
  %495 = tail call double @sin(double noundef %490) #10
  %496 = tail call double @cos(double noundef %490) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %47) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %48) #10
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %49) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %50) #10
  %497 = fmul double %492, %494
  store double %497, ptr %50, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %51) #10
  %498 = fmul double %491, %494
  store double %498, ptr %51, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %52) #10
  %499 = fneg double %493
  store double %499, ptr %52, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %53) #10
  %500 = fmul double %492, %493
  %501 = fneg double %491
  %502 = fmul double %496, %501
  %503 = tail call double @llvm.fmuladd.f64(double %500, double %495, double %502)
  store double %503, ptr %53, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %54) #10
  %504 = fmul double %491, %493
  %505 = fmul double %492, %496
  %506 = tail call double @llvm.fmuladd.f64(double %504, double %495, double %505)
  store double %506, ptr %54, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %55) #10
  %507 = fmul double %494, %495
  store double %507, ptr %55, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %56) #10
  %508 = fmul double %491, %495
  %509 = tail call double @llvm.fmuladd.f64(double %500, double %496, double %508)
  store double %509, ptr %56, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %57) #10
  %510 = fneg double %492
  %511 = fmul double %495, %510
  %512 = tail call double @llvm.fmuladd.f64(double %504, double %496, double %511)
  store double %512, ptr %57, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %58) #10
  %513 = fmul double %494, %496
  store double %513, ptr %58, align 8, !tbaa !13
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %49, ptr noundef nonnull align 8 dereferenceable(8) %50, ptr noundef nonnull align 8 dereferenceable(8) %51, ptr noundef nonnull align 8 dereferenceable(8) %52, ptr noundef nonnull align 8 dereferenceable(8) %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, ptr noundef nonnull align 8 dereferenceable(8) %57, ptr noundef nonnull align 8 dereferenceable(8) %58)
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %59) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %60) #10
  store double 0.000000e+00, ptr %60, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %61) #10
  store double 0.000000e+00, ptr %61, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %62) #10
  store double 0.000000e+00, ptr %62, align 8, !tbaa !13
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %59, ptr noundef nonnull align 8 dereferenceable(8) %60, ptr noundef nonnull align 8 dereferenceable(8) %61, ptr noundef nonnull align 8 dereferenceable(8) %62)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %48, ptr noundef nonnull align 8 dereferenceable(72) %49, ptr noundef nonnull align 8 dereferenceable(24) %59)
  %514 = getelementptr inbounds i8, ptr %0, i64 280
  %515 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %514, i64 noundef %120) #10
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %47, ptr noundef nonnull align 8 dereferenceable(96) %48, ptr noundef nonnull align 8 dereferenceable(96) %515)
  %516 = getelementptr inbounds i8, ptr %0, i64 720
  %517 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %516, i64 noundef %120) #10
  %518 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %517, ptr noundef nonnull align 8 dereferenceable(96) %47) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %62) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %61) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %60) #10
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %59) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %58) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %57) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %56) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %55) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %54) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %53) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %52) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %51) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %50) #10
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %49) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %48) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %47) #10
  %519 = getelementptr inbounds i8, ptr %0, i64 328
  %520 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %519, i64 noundef %120) #10
  %521 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %520, i64 noundef 0, i64 noundef 0)
  store double %499, ptr %521, align 8, !tbaa !13
  %522 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %519, i64 noundef %120) #10
  %523 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %522, i64 noundef 0, i64 noundef 2)
  store double 1.000000e+00, ptr %523, align 8, !tbaa !13
  %524 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %519, i64 noundef %120) #10
  %525 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %524, i64 noundef 1, i64 noundef 0)
  store double %507, ptr %525, align 8, !tbaa !13
  %526 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %519, i64 noundef %120) #10
  %527 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %526, i64 noundef 1, i64 noundef 1)
  store double %496, ptr %527, align 8, !tbaa !13
  %528 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %519, i64 noundef %120) #10
  %529 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %528, i64 noundef 2, i64 noundef 0)
  store double %513, ptr %529, align 8, !tbaa !13
  %530 = fneg double %495
  %531 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %519, i64 noundef %120) #10
  %532 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %531, i64 noundef 2, i64 noundef 1)
  store double %530, ptr %532, align 8, !tbaa !13
  br label %793

533:                                              ; preds = %465
  %534 = getelementptr inbounds i8, ptr %470, i64 8
  %535 = load i32, ptr %534, align 8, !tbaa !5
  %536 = icmp eq i32 %535, 8
  %537 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  br i1 %536, label %538, label %602

538:                                              ; preds = %533
  %539 = getelementptr inbounds i8, ptr %537, i64 16
  %540 = load i32, ptr %539, align 8, !tbaa !12
  %541 = zext i32 %540 to i64
  %542 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %541)
  %543 = load double, ptr %542, align 8, !tbaa !13
  %544 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %545 = getelementptr inbounds i8, ptr %544, i64 16
  %546 = load i32, ptr %545, align 8, !tbaa !12
  %547 = add i32 %546, 1
  %548 = zext i32 %547 to i64
  %549 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %548)
  %550 = load double, ptr %549, align 8, !tbaa !13
  %551 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %552 = getelementptr inbounds i8, ptr %551, i64 16
  %553 = load i32, ptr %552, align 8, !tbaa !12
  %554 = add i32 %553, 2
  %555 = zext i32 %554 to i64
  %556 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %555)
  %557 = load double, ptr %556, align 8, !tbaa !13
  %558 = tail call double @sin(double noundef %543) #10
  %559 = tail call double @cos(double noundef %543) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %63) #10
  %560 = tail call double @sin(double noundef %550) #10
  store double %560, ptr %63, align 8, !tbaa !13
  %561 = tail call double @cos(double noundef %550) #10
  %562 = tail call double @sin(double noundef %557) #10
  %563 = tail call double @cos(double noundef %557) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %64) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %65) #10
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %66) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %67) #10
  %564 = fmul double %561, %563
  store double %564, ptr %67, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %68) #10
  %565 = fmul double %563, %560
  %566 = fmul double %558, %565
  %567 = tail call double @llvm.fmuladd.f64(double %562, double %559, double %566)
  store double %567, ptr %68, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %69) #10
  %568 = fneg double %563
  %569 = fmul double %560, %568
  %570 = fmul double %559, %569
  %571 = tail call double @llvm.fmuladd.f64(double %562, double %558, double %570)
  store double %571, ptr %69, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %70) #10
  %572 = fneg double %562
  %573 = fmul double %561, %572
  store double %573, ptr %70, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %71) #10
  %574 = fmul double %560, %572
  %575 = fmul double %558, %574
  %576 = tail call double @llvm.fmuladd.f64(double %563, double %559, double %575)
  store double %576, ptr %71, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %72) #10
  %577 = fmul double %562, %560
  %578 = fmul double %559, %577
  %579 = tail call double @llvm.fmuladd.f64(double %563, double %558, double %578)
  store double %579, ptr %72, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %73) #10
  %580 = fneg double %561
  %581 = fmul double %558, %580
  store double %581, ptr %73, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %74) #10
  %582 = fmul double %559, %561
  store double %582, ptr %74, align 8, !tbaa !13
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %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) %63, ptr noundef nonnull align 8 dereferenceable(8) %73, ptr noundef nonnull align 8 dereferenceable(8) %74)
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %75) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %76) #10
  store double 0.000000e+00, ptr %76, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %77) #10
  store double 0.000000e+00, ptr %77, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %78) #10
  store double 0.000000e+00, ptr %78, align 8, !tbaa !13
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %75, ptr noundef nonnull align 8 dereferenceable(8) %76, ptr noundef nonnull align 8 dereferenceable(8) %77, ptr noundef nonnull align 8 dereferenceable(8) %78)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %65, ptr noundef nonnull align 8 dereferenceable(72) %66, ptr noundef nonnull align 8 dereferenceable(24) %75)
  %583 = getelementptr inbounds i8, ptr %0, i64 280
  %584 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %583, i64 noundef %120) #10
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %64, ptr noundef nonnull align 8 dereferenceable(96) %65, ptr noundef nonnull align 8 dereferenceable(96) %584)
  %585 = getelementptr inbounds i8, ptr %0, i64 720
  %586 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %585, i64 noundef %120) #10
  %587 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %586, ptr noundef nonnull align 8 dereferenceable(96) %64) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %78) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %77) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %76) #10
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %75) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %74) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %73) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %72) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %71) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %70) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %69) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %68) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %67) #10
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %66) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %65) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %64) #10
  %588 = getelementptr inbounds i8, ptr %0, i64 328
  %589 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %588, i64 noundef %120) #10
  %590 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %589, i64 noundef 0, i64 noundef 0)
  store double %564, ptr %590, align 8, !tbaa !13
  %591 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %588, i64 noundef %120) #10
  %592 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %591, i64 noundef 0, i64 noundef 1)
  store double %562, ptr %592, align 8, !tbaa !13
  %593 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %588, i64 noundef %120) #10
  %594 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %593, i64 noundef 1, i64 noundef 0)
  store double %573, ptr %594, align 8, !tbaa !13
  %595 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %588, i64 noundef %120) #10
  %596 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %595, i64 noundef 1, i64 noundef 1)
  store double %563, ptr %596, align 8, !tbaa !13
  %597 = load double, ptr %63, align 8, !tbaa !13
  %598 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %588, i64 noundef %120) #10
  %599 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %598, i64 noundef 2, i64 noundef 0)
  store double %597, ptr %599, align 8, !tbaa !13
  %600 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %588, i64 noundef %120) #10
  %601 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %600, i64 noundef 2, i64 noundef 2)
  store double 1.000000e+00, ptr %601, align 8, !tbaa !13
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %63) #10
  br label %793

602:                                              ; preds = %533
  %603 = getelementptr inbounds i8, ptr %537, i64 8
  %604 = load i32, ptr %603, align 8, !tbaa !5
  %605 = icmp eq i32 %604, 9
  %606 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  br i1 %605, label %607, label %668

607:                                              ; preds = %602
  %608 = getelementptr inbounds i8, ptr %606, i64 16
  %609 = load i32, ptr %608, align 8, !tbaa !12
  %610 = zext i32 %609 to i64
  %611 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %610)
  %612 = load double, ptr %611, align 8, !tbaa !13
  %613 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %614 = getelementptr inbounds i8, ptr %613, i64 16
  %615 = load i32, ptr %614, align 8, !tbaa !12
  %616 = add i32 %615, 1
  %617 = zext i32 %616 to i64
  %618 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %617)
  %619 = load double, ptr %618, align 8, !tbaa !13
  %620 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %621 = getelementptr inbounds i8, ptr %620, i64 16
  %622 = load i32, ptr %621, align 8, !tbaa !12
  %623 = add i32 %622, 2
  %624 = zext i32 %623 to i64
  %625 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %624)
  %626 = load double, ptr %625, align 8, !tbaa !13
  %627 = tail call double @sin(double noundef %612) #10
  %628 = tail call double @cos(double noundef %612) #10
  %629 = tail call double @sin(double noundef %619) #10
  %630 = tail call double @cos(double noundef %619) #10
  %631 = tail call double @sin(double noundef %626) #10
  %632 = tail call double @cos(double noundef %626) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %79) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %80) #10
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %81) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %82) #10
  %633 = fmul double %629, %631
  %634 = fmul double %627, %633
  %635 = tail call double @llvm.fmuladd.f64(double %632, double %628, double %634)
  store double %635, ptr %82, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %83) #10
  %636 = fmul double %630, %631
  store double %636, ptr %83, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %84) #10
  %637 = fneg double %632
  %638 = fmul double %628, %633
  %639 = tail call double @llvm.fmuladd.f64(double %637, double %627, double %638)
  store double %639, ptr %84, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %85) #10
  %640 = fneg double %631
  %641 = fmul double %629, %632
  %642 = fmul double %627, %641
  %643 = tail call double @llvm.fmuladd.f64(double %640, double %628, double %642)
  store double %643, ptr %85, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %86) #10
  %644 = fmul double %630, %632
  store double %644, ptr %86, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %87) #10
  %645 = fmul double %628, %641
  %646 = tail call double @llvm.fmuladd.f64(double %631, double %627, double %645)
  store double %646, ptr %87, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %88) #10
  %647 = fmul double %627, %630
  store double %647, ptr %88, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %89) #10
  %648 = fneg double %629
  store double %648, ptr %89, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %90) #10
  %649 = fmul double %628, %630
  store double %649, ptr %90, align 8, !tbaa !13
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %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, ptr noundef nonnull align 8 dereferenceable(8) %87, ptr noundef nonnull align 8 dereferenceable(8) %88, ptr noundef nonnull align 8 dereferenceable(8) %89, ptr noundef nonnull align 8 dereferenceable(8) %90)
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %91) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %92) #10
  store double 0.000000e+00, ptr %92, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %93) #10
  store double 0.000000e+00, ptr %93, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %94) #10
  store double 0.000000e+00, ptr %94, align 8, !tbaa !13
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %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)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %80, ptr noundef nonnull align 8 dereferenceable(72) %81, ptr noundef nonnull align 8 dereferenceable(24) %91)
  %650 = getelementptr inbounds i8, ptr %0, i64 280
  %651 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %650, i64 noundef %120) #10
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %79, ptr noundef nonnull align 8 dereferenceable(96) %80, ptr noundef nonnull align 8 dereferenceable(96) %651)
  %652 = getelementptr inbounds i8, ptr %0, i64 720
  %653 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %652, i64 noundef %120) #10
  %654 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %653, ptr noundef nonnull align 8 dereferenceable(96) %79) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %94) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %93) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %92) #10
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %91) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %90) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %89) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %88) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %87) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %86) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %85) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %84) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %83) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %82) #10
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %81) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %80) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %79) #10
  %655 = getelementptr inbounds i8, ptr %0, i64 328
  %656 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %655, i64 noundef %120) #10
  %657 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %656, i64 noundef 0, i64 noundef 0)
  store double %636, ptr %657, align 8, !tbaa !13
  %658 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %655, i64 noundef %120) #10
  %659 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %658, i64 noundef 0, i64 noundef 1)
  store double %632, ptr %659, align 8, !tbaa !13
  %660 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %655, i64 noundef %120) #10
  %661 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %660, i64 noundef 1, i64 noundef 0)
  store double %644, ptr %661, align 8, !tbaa !13
  %662 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %655, i64 noundef %120) #10
  %663 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %662, i64 noundef 1, i64 noundef 1)
  store double %640, ptr %663, align 8, !tbaa !13
  %664 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %655, i64 noundef %120) #10
  %665 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %664, i64 noundef 2, i64 noundef 0)
  store double %648, ptr %665, align 8, !tbaa !13
  %666 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %655, i64 noundef %120) #10
  %667 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %666, i64 noundef 2, i64 noundef 2)
  store double 1.000000e+00, ptr %667, align 8, !tbaa !13
  br label %793

668:                                              ; preds = %602
  %669 = getelementptr inbounds i8, ptr %606, i64 8
  %670 = load i32, ptr %669, align 8, !tbaa !5
  %671 = icmp eq i32 %670, 10
  %672 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  br i1 %671, label %673, label %737

673:                                              ; preds = %668
  %674 = getelementptr inbounds i8, ptr %672, i64 16
  %675 = load i32, ptr %674, align 8, !tbaa !12
  %676 = zext i32 %675 to i64
  %677 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %676)
  %678 = load double, ptr %677, align 8, !tbaa !13
  %679 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %680 = getelementptr inbounds i8, ptr %679, i64 16
  %681 = load i32, ptr %680, align 8, !tbaa !12
  %682 = add i32 %681, 1
  %683 = zext i32 %682 to i64
  %684 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %683)
  %685 = load double, ptr %684, align 8, !tbaa !13
  %686 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %687 = getelementptr inbounds i8, ptr %686, i64 16
  %688 = load i32, ptr %687, align 8, !tbaa !12
  %689 = add i32 %688, 2
  %690 = zext i32 %689 to i64
  %691 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %690)
  %692 = load double, ptr %691, align 8, !tbaa !13
  %693 = tail call double @sin(double noundef %678) #10
  %694 = tail call double @cos(double noundef %678) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %95) #10
  %695 = tail call double @sin(double noundef %685) #10
  store double %695, ptr %95, align 8, !tbaa !13
  %696 = tail call double @cos(double noundef %685) #10
  %697 = tail call double @sin(double noundef %692) #10
  %698 = tail call double @cos(double noundef %692) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %96) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %97) #10
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %98) #10
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %99) #10
  %699 = fneg double %693
  %700 = fmul double %695, %699
  %701 = fmul double %694, %698
  %702 = tail call double @llvm.fmuladd.f64(double %700, double %697, double %701)
  store double %702, ptr %99, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %100) #10
  %703 = fmul double %697, %695
  %704 = fmul double %694, %703
  %705 = tail call double @llvm.fmuladd.f64(double %693, double %698, double %704)
  store double %705, ptr %100, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %101) #10
  %706 = fneg double %697
  %707 = fmul double %696, %706
  store double %707, ptr %101, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %102) #10
  %708 = fmul double %696, %699
  store double %708, ptr %102, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %103) #10
  %709 = fmul double %694, %696
  store double %709, ptr %103, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %104) #10
  %710 = fmul double %693, %695
  %711 = fmul double %694, %697
  %712 = tail call double @llvm.fmuladd.f64(double %710, double %698, double %711)
  store double %712, ptr %104, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %105) #10
  %713 = fneg double %695
  %714 = fmul double %694, %713
  %715 = fmul double %698, %714
  %716 = tail call double @llvm.fmuladd.f64(double %693, double %697, double %715)
  store double %716, ptr %105, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %106) #10
  %717 = fmul double %696, %698
  store double %717, ptr %106, align 8, !tbaa !13
  call void @_ZN9Matrix3_tC2ERKdS1_S1_S1_S1_S1_S1_S1_S1_(ptr noundef nonnull align 8 dereferenceable(72) %98, ptr noundef nonnull align 8 dereferenceable(8) %99, ptr noundef nonnull align 8 dereferenceable(8) %100, ptr noundef nonnull align 8 dereferenceable(8) %101, ptr noundef nonnull align 8 dereferenceable(8) %102, ptr noundef nonnull align 8 dereferenceable(8) %103, ptr noundef nonnull align 8 dereferenceable(8) %95, ptr noundef nonnull align 8 dereferenceable(8) %104, ptr noundef nonnull align 8 dereferenceable(8) %105, ptr noundef nonnull align 8 dereferenceable(8) %106)
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %107) #10
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %108) #10
  call void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE4ZeroEv(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseNullaryOp.149") align 8 %108)
  call void @_ZN9Vector3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %107, ptr noundef nonnull align 1 dereferenceable(1) %108)
  call void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2ERK9Matrix3_tRK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(96) %97, ptr noundef nonnull align 8 dereferenceable(72) %98, ptr noundef nonnull align 8 dereferenceable(24) %107)
  %718 = getelementptr inbounds i8, ptr %0, i64 280
  %719 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %718, i64 noundef %120) #10
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %96, ptr noundef nonnull align 8 dereferenceable(96) %97, ptr noundef nonnull align 8 dereferenceable(96) %719)
  %720 = getelementptr inbounds i8, ptr %0, i64 720
  %721 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %720, i64 noundef %120) #10
  %722 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %721, ptr noundef nonnull align 8 dereferenceable(96) %96) #10
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %108) #10
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %107) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %106) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %105) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %104) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %103) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %102) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %101) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %100) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %99) #10
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %98) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %97) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %96) #10
  %723 = getelementptr inbounds i8, ptr %0, i64 328
  %724 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %723, i64 noundef %120) #10
  %725 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %724, i64 noundef 0, i64 noundef 0)
  store double %707, ptr %725, align 8, !tbaa !13
  %726 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %723, i64 noundef %120) #10
  %727 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %726, i64 noundef 0, i64 noundef 1)
  store double %698, ptr %727, align 8, !tbaa !13
  %728 = load double, ptr %95, align 8, !tbaa !13
  %729 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %723, i64 noundef %120) #10
  %730 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %729, i64 noundef 1, i64 noundef 0)
  store double %728, ptr %730, align 8, !tbaa !13
  %731 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %723, i64 noundef %120) #10
  %732 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %731, i64 noundef 1, i64 noundef 2)
  store double 1.000000e+00, ptr %732, align 8, !tbaa !13
  %733 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %723, i64 noundef %120) #10
  %734 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %733, i64 noundef 2, i64 noundef 0)
  store double %717, ptr %734, align 8, !tbaa !13
  %735 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %723, i64 noundef %120) #10
  %736 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %735, i64 noundef 2, i64 noundef 1)
  store double %697, ptr %736, align 8, !tbaa !13
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %95) #10
  br label %793

737:                                              ; preds = %668
  %738 = getelementptr inbounds i8, ptr %672, i64 8
  %739 = load i32, ptr %738, align 8, !tbaa !5
  %740 = icmp eq i32 %739, 11
  br i1 %740, label %741, label %774

741:                                              ; preds = %737
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %109) #10
  %742 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %743 = getelementptr inbounds i8, ptr %742, i64 16
  %744 = load i32, ptr %743, align 8, !tbaa !12
  %745 = zext i32 %744 to i64
  %746 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %745)
  %747 = load double, ptr %746, align 8, !tbaa !13
  store double %747, ptr %109, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %110) #10
  %748 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %749 = getelementptr inbounds i8, ptr %748, i64 16
  %750 = load i32, ptr %749, align 8, !tbaa !12
  %751 = add i32 %750, 1
  %752 = zext i32 %751 to i64
  %753 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %752)
  %754 = load double, ptr %753, align 8, !tbaa !13
  store double %754, ptr %110, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %111) #10
  %755 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %756 = getelementptr inbounds i8, ptr %755, i64 16
  %757 = load i32, ptr %756, align 8, !tbaa !12
  %758 = add i32 %757, 2
  %759 = zext i32 %758 to i64
  %760 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNK5Eigen15DenseCoeffsBaseINS_6MatrixIdLin1ELi1ELi0ELin1ELi1EEELi0EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %2, i64 noundef %759)
  %761 = load double, ptr %760, align 8, !tbaa !13
  store double %761, ptr %111, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %112) #10
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %113) #10
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %114) #10
  call void @llvm.lifetime.start.p0(i64 3, ptr nonnull %115) #10
  call void @_ZN5Eigen10MatrixBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE8IdentityEll(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseNullaryOp.190") align 1 %115, i64 noundef 3, i64 noundef 3)
  call void @_ZN9Matrix3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_identity_opIdEENS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(72) %114, ptr noundef nonnull align 1 dereferenceable(1) %115)
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %116) #10
  call void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %116, 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 @_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) %116)
  %762 = getelementptr inbounds i8, ptr %0, i64 280
  %763 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %762, i64 noundef %120) #10
  call void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %112, ptr noundef nonnull align 8 dereferenceable(96) %113, ptr noundef nonnull align 8 dereferenceable(96) %763)
  %764 = getelementptr inbounds i8, ptr %0, i64 720
  %765 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZNSt6vectorIN17RigidBodyDynamics4Math16SpatialTransformEN5Eigen17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %764, i64 noundef %120) #10
  %766 = call noundef nonnull align 8 dereferenceable(96) ptr @_ZN17RigidBodyDynamics4Math16SpatialTransformaSEOS1_(ptr noundef nonnull align 8 dereferenceable(96) %765, ptr noundef nonnull align 8 dereferenceable(96) %112) #10
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %116) #10
  call void @llvm.lifetime.end.p0(i64 3, ptr nonnull %115) #10
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %114) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %113) #10
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %112) #10
  %767 = getelementptr inbounds i8, ptr %0, i64 328
  %768 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %767, i64 noundef %120) #10
  %769 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %768, i64 noundef 3, i64 noundef 0)
  store double 1.000000e+00, ptr %769, align 8, !tbaa !13
  %770 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %767, i64 noundef %120) #10
  %771 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %770, i64 noundef 4, i64 noundef 1)
  store double 1.000000e+00, ptr %771, align 8, !tbaa !13
  %772 = call noundef nonnull align 16 dereferenceable(144) ptr @_ZNSt6vectorIN5Eigen6MatrixIdLi6ELi3ELi0ELi6ELi3EEENS0_17aligned_allocatorIS2_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %767, i64 noundef %120) #10
  %773 = call noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi6ELi3ELi0ELi6ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %772, i64 noundef 5, i64 noundef 2)
  store double 1.000000e+00, ptr %773, align 8, !tbaa !13
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %111) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %110) #10
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %109) #10
  br label %793

774:                                              ; preds = %737
  %775 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %776 = getelementptr inbounds i8, ptr %775, i64 8
  %777 = load i32, ptr %776, align 8, !tbaa !5
  %778 = icmp eq i32 %777, 21
  br i1 %778, label %779, label %790

779:                                              ; preds = %774
  %780 = tail call noundef nonnull align 8 dereferenceable(24) ptr @_ZNSt6vectorIN17RigidBodyDynamics5JointEN5Eigen17aligned_allocatorIS1_EEEixEm(ptr noundef nonnull align 8 dereferenceable(24) %119, i64 noundef %120) #10
  %781 = getelementptr inbounds i8, ptr %0, i64 448
  %782 = getelementptr inbounds i8, ptr %780, i64 20
  %783 = load i32, ptr %782, align 4, !tbaa !17
  %784 = sext i32 %783 to i64
  %785 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSt6vectorIPN17RigidBodyDynamics11CustomJointESaIS2_EEixEm(ptr noundef nonnull align 8 dereferenceable(24) %781, i64 noundef %784) #10
  %786 = load ptr, ptr %785, align 8, !tbaa !18
  %787 = load ptr, ptr %786, align 8, !tbaa !19
  %788 = getelementptr inbounds i8, ptr %787, i64 24
  %789 = load ptr, ptr %788, align 8
  tail call void %789(ptr noundef nonnull align 8 dereferenceable(216) %786, ptr noundef nonnull align 8 dereferenceable(872) %0, i32 noundef %1, ptr noundef nonnull align 8 dereferenceable(16) %2)
  br label %793

790:                                              ; preds = %774
  %791 = 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.8)
  %792 = tail call noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %791, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
  tail call void @abort() #9
  unreachable

793:                                              ; preds = %218, %404, %452, %538, %673, %779, %741, %607, %471, %436, %311, %125
  ret void
}

; 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 #4 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 #4 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 #7 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 #4 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 #4 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 #6 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 #7 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 #6 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 #7 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 #7 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 #4 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 #7 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 #1 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 #4 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 #4 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 #7 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 #1 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 #4

; 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 #7 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)) #4

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

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

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen10MatrixBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE8IdentityEll(ptr dead_on_unwind noalias writable sret(%"class.Eigen::CwiseNullaryOp.190") align 1, i64 noundef, i64 noundef) local_unnamed_addr #4 align 2

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

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