Translate XNA code to Torque3D

Vector3 s;
            Quaternion r;
            Vector3 t;
            if (!worldTransform.Decompose(out s, out r, out t))
                return;

            var currentPos = positionResolver(currentJointID);
            var nextPos = positionResolver(nextJointID);

            var shoulderToElbowDir = Vector3.Normalize(nextPos - currentPos);

            var angle = (float)Math.Acos(Vector3.Dot(Vector3.Right, shoulderToElbowDir));
            var axis = Vector3.Normalize(Vector3.Cross(Vector3.Right, shoulderToElbowDir));

            worldTransform = Matrix.CreateScale(s) * Matrix.CreateFromAxisAngle(axis, angle) * Matrix.CreateTranslation(t);

MatrixF mScale(true);
			mScale.scale(WorldTransform->getScale());
			
            MatrixF mPosition(true);
			mPosition.setPosition(WorldTransform->getPosition());
			
		
			//Calculate Joint Direction			
            Point3F shoulderToElbowDir = mNormalize(NextPos - CurrentPos);
			Point3F RightVec = MatrixF(true).getRightVector();
			//Calculate Angle						
            F32 angle = acos(mDot(RightVec, shoulderToElbowDir));
			//Calculate Axis
            Point3F axis = mNormalize(mCross(RightVec, shoulderToElbowDir));
			//Create AngAxis rotation
			AngAxisF aa(axis,angle);
			MatrixF RotMat;
			aa.setMatrix(&RotMat);

           WorldTransform = &(mScale * RotMat * mPosition);