Простой трассировщик лучей, проблемы с диффузным затенением С++

Я пишу базовый трассировщик лучей, чтобы лучше понять все это. Я столкнулся с проблемой, которая некоторое время сдерживала меня: диффузное затенение сферы. Я использовал формулу из следующего источника для расчета пересечений сфер и диффузного затенения.

http://www.ccs.neu.edu/home/fell/CSU540/programs/RayTracingFormulas.htm

Мой код, который вычисляет затенение (попытка репликации исходного кода по ссылке), показан выше. По большей части вычисления кажутся правильными для некоторых сфер, время от времени, однако в зависимости от положения источников света зависит, насколько правильным/неправильным выглядит затенение сфер.

TVector intersect   (ray.getRayOrigin().getVectX() + t * (ray.getRayDirection().getVectX() - ray.getRayOrigin().getVectX()),
                    ray.getRayOrigin().getVectY() + t * (ray.getRayDirection().getVectY() - ray.getRayOrigin().getVectY()),
                    ray.getRayOrigin().getVectZ() + t * (ray.getRayDirection().getVectZ() - ray.getRayOrigin().getVectZ()));

//Calculate the normal at the intersect point
TVector NormalIntersect (intersect.getVectX() - (position.getVectX()/r), 
                        intersect.getVectY() - (position.getVectY()/r),
                        intersect.getVectZ() - (position.getVectZ()/r));

NormalIntersect = NormalIntersect.normalize();

//Find unit vector from intersect(x,y,z) to the light(x,y,z)
TVector L1 (light.GetPosition().getVectX() - intersect.getVectX(), 
            light.GetPosition().getVectY() - intersect.getVectY(),
            light.GetPosition().getVectZ() - intersect.getVectZ());
L1 = L1.normalize();
double Magnitude = L1.magnitude();

TVector UnitVector(L1.getVectX() / Magnitude,
                   L1.getVectY() / Magnitude,
                   L1.getVectZ() / Magnitude);

//Normalized or not, the result is the same
UnitVector = UnitVector.normalize();

float Factor = (NormalIntersect.dotProduct(UnitVector));
float kd = 0.9;             //diffuse-coefficient
float ka = 0.1;             //Ambient-coefficient
Color pixelFinalColor(kd * Factor * (color.getcolorRed())  +  (ka * color.getcolorRed()) ,
                      kd * Factor * (color.getcolorGreen())  + (ka * color.getcolorGreen())  ,
                      kd * Factor * (color.getcolorBlue()) +  (ka * color.getcolorBlue()) ,1);

Как видно на картинке, некоторые сферы заштрихованы правильно, а другие полностью сломаны. Сначала я подумал, что проблема может заключаться в вычислении UnitVector, однако, когда я просмотрел его, я не смог найти проблему. Может ли кто-нибудь увидеть причину проблемы?

Примечание. Я использую OpenGl для рендеринга сцены.

Обновление: у меня все еще есть несколько проблем, однако я думаю, что они в основном были решены благодаря вашей помощи, ребята, и нескольким изменениям в том, как я вычисляю единичный вектор. Обновления показаны ниже. Большое спасибо всем, кто дал свои ответы.

TVector UnitVector (light.GetPosition().getVectX() - intersect.getVectX(), 
                    light.GetPosition().getVectY() - intersect.getVectY(),
                    light.GetPosition().getVectZ() - intersect.getVectZ());

UnitVector = UnitVector.normalize();
float Factor = NormalIntersect.dotProduct(UnitVector);

//Set Pixel Final Color
Color pixelFinalColor(min(1,kd * Factor * color.getcolorRed())  +  (ka * color.getcolorRed()) ,
                      min(1,kd * Factor * color.getcolorGreen())  + (ka * color.getcolorGreen())  ,
                      min(1,kd * Factor * color.getcolorBlue()) +  (ka * color.getcolorBlue()) ,1);