LOCAL PROC PathInterpolation(robtarget p1, robtarget p2, INOUT robtarget pInterpolatedIN{*}, PERS tooldata tToolToUse \PERS wobjdata wobjToUse )
        !Support variables
        VAR robtarget dnResult;
        VAR robtarget dnResult2;
        VAR num count := 1;
        VAR num t:= 0;
        VAR jointtarget jResult;
        VAR jointtarget jResultprev:=jResult;
        
        WHILE count<=nSize DO
        t:= (count-1)*ndt;
        
        ! Linear interpolation for positions
        dnResult.trans.x := (1 - t) * p1.trans.x + t * p2.trans.x;
        dnResult.trans.y := (1 - t) * p1.trans.y + t * p2.trans.y;
        dnResult.trans.z := (1 - t) * p1.trans.z + t * p2.trans.z;
        
        ! Slerp interpolation for orientations
        dnResult.rot := Slerp(p1.rot, p2.rot, t);
        
        ! Copy the other attributes
        dnResult.robconf := p1.robconf;
        dnResult.extax := p1.extax;        ! DEBUG
        !TPWrite "t: "+ NumToStr(t,5) + "count: " + NumToStr(count,1);
        
        ! Save in the array
        pInterpolatedIN{count} := dnResult;
        Incr count;
        
        ENDWHILE
        ! Ensure the last point exactly matches p2
        dnResult := p2;
        pInterpolatedIN{nSize} := dnResult; ! Ensure the last point is exactly p2
        
    ENDPROC

    !Slerp : Perform SLERP interpolation for ORIENTATION
    LOCAL FUNC orient Slerp(orient p1, orient p2, num t)
        ! Support variables
        VAR num dot ;
        VAR num n_theta;
        VAR num dnSinTheta;
        VAR num scale1;
        VAR num scale2;
        VAR orient dnResult;
        VAR num magnitude;
        
        ! Compute the dot product (cosine of the angle between the quaternions)
        dot := p1.q1*p2.q1 + p1.q2*p2.q2 + p1.q3*p2.q3 + p1.q4*p2.q4;
        
        ! If the dot product is negative, negate one quaternion to take the shorter path
        IF dot < 0 THEN
            p1.q1 := -p1.q1;
            p1.q2 := -p1.q2;
            p1.q3 := -p1.q3;
            p1.q4 := -p1.q4;
            dot := -dot;
        ENDIF
    
        ! If the quaternions are too close, use linear interpolation
        IF dot > 0.9995 THEN
            dnResult.q1 := (1 - t) * p1.q1 + t * p2.q1;
            dnResult.q2 := (1 - t) * p1.q2 + t * p2.q2;
            dnResult.q3 := (1 - t) * p1.q3 + t * p2.q3;
            dnResult.q4 := (1 - t) * p1.q4 + t * p2.q4;
        ELSE
            ! Compute the angle and sin(n_theta)
            n_theta := ACOS(dot);
            dnSinTheta := SIN(n_theta);
            
            ! Compute the scaling factors
            scale1 := SIN((1 - t) * n_theta) / dnSinTheta;
            scale2 := SIN(t * n_theta) / dnSinTheta;
            
            ! Compute the interpolated quaternion
            dnResult.q1 := scale1 * p1.q1 + scale2 * p2.q1;
            dnResult.q2 := scale1 * p1.q2 + scale2 * p2.q2;
            dnResult.q3 := scale1 * p1.q3 + scale2 * p2.q3;
            dnResult.q4 := scale1 * p1.q4 + scale2 * p2.q4;
        ENDIF
        
        ! Normalize the resulting quaternion
        magnitude := SQRT(dnResult.q1*dnResult.q1 + dnResult.q2*dnResult.q2 + dnResult.q3*dnResult.q3 + dnResult.q4*dnResult.q4);
        dnResult.q1 := dnResult.q1 / magnitude;
        dnResult.q2 := dnResult.q2 / magnitude;
        dnResult.q3 := dnResult.q3 / magnitude;
        dnResult.q4 := dnResult.q4 / magnitude;
        
        ! Return the normalized result
        RETURN dnResult;
    ENDFUNC