Не пишите это самостоятельно, не занимайтесь попытками создать чей-либо источник.Используйте библиотеку, которая предоставляет эту функцию для вас.Возможно, на вашей целевой платформе уже есть такая.
В частности, используйте стандартную библиотеку LAPACK или GSL или любую другую библиотеку линейной алгебры, которую вы хотите.Все они имеют реализацию SVD.