classdef RodConstraint
    
    properties
        % Index of the body at the beginning of the rod.
        indexA;
        % Index of the body at the end of the rod.
        indexB;
        
        % Material frames for each body.
        % This is a quaternion representing the rotation from the body
        % frame to material frame.
        F_a=[1 0 0 0]';
        F_b=[1 0 0 0]';
        
        % The rod will resist any bending beyond the hard limit with
        % infinite strength. This is the limit of the angle psi, which is
        % the angle between each body tangent and the offset.
        bendHardLimit=pi/2;
        % The rod will resist bending beyond the soft limit with strength
        % proportional to alpha_b.
        bendSoftLimit=0;
        alpha_b=0.10;
        
        % Same deal with twisting motion. This is the angle tau, which is
        % the total angle between the two body frames minus the bending
        % angle
        twistHardLimit=pi/2;
        twistSoftLimit=0;
        alpha_t=0.00;
        
        % Rest length of the rod.
        l_0=1;
    end
    
    methods
        % Given body states at the beginning of the frame and all impulses
        % applied to the bodies, finds updates to the impulses that will
        % cause the constraint to be solved.
         function [aSolved bSolved] = Solve(constraint,a,b)
            
            % A note on how we solve this constraint:
            % While we can use the standard method involving a
            % linear system of the form JM^-1J' a = b, there are some
            % issues with this method. First of all, there are 7
            % constraints in total - 5 equality and 2 inequality - and
            % inverting 4 7x7 matrices at once with SIMD is a pain.
            % Second, the Jacobian for the curve constraint is singular at
            % certain common configurations (for instance, when t_a = t_b).
            % We could detect these situations and modify the Jacobian to
            % eliminate the singularity (choosing a direction arbitrarily),
            % but instead we just solve the constraints directly with
            % velocity updates.
            
            % Specifically, we decompose the curve constraint into a twist
            % of the two bodies about oHat (purely to satisfy the condition
            % that t_a, t_b and oHat must be in the same plane) and then a
            % wrench about the curve normal to ensure the angles the two
            % tangents make with oHat are equal. We then solve the length
            % constraint purely by moving the two bodies away or towards
            % each other. We can solve the bend and twist constraints
            % easily because the system is in 2D once the previous
            % constraints have been satisfied. We have to do a little
            % integration between constraints to ensure decent convergence,
            % but we can use a cheap small angle approximation for this because
            % our updates to the velocity will be infinitesimal.
            
            x = [1 0 0]';
            y = [0 1 0]';
            z = [0 0 1]';
            
            % Compute spherical moment of inertia. It's a common
            % optimisation to use spherical inertias for ragdoll parts
            % because it stabilises their motion. It also makes the
            % constraint a bit cheaper to solve. Note that this is NOT
            % a necessary simplification to solve the constraint in this
            % way.
            inverseSphericalInertiaA = a.inverseMass*norm(a.unitMassInverseInertia);
            inverseSphericalInertiaB = b.inverseMass*norm(b.unitMassInverseInertia);
            
            % Generate preview positions
            p_a = a.position + a.displacement;
            p_b = b.position + b.displacement;
            
            % Generate preview material frames.
            materialA = qMult(quaternionFromAxisAngle(a.angularDisplacement),qMult(a.orientation,constraint.F_a));
            materialB = qMult(quaternionFromAxisAngle(b.angularDisplacement),qMult(b.orientation,constraint.F_b));
            
            % Find body tangents and normals from the material frames.
            t_a = transformVector(materialA,x);
            t_b = transformVector(materialB,x);
            n_a = transformVector(materialA,z);
            n_b = transformVector(materialB,z);
            
            initial_p_a = p_a;
            initial_p_b = p_b;
            initial_t_a = t_a;
            initial_t_b = t_b;
            
            offset = p_b - p_a;
            l = norm(offset);
            oHat = offset/l;

            % Compute reduced mass
            m_red = 1/(a.inverseMass + b.inverseMass);
            % Compute mass weightings
            % = each body's proportion of any given linear impulse
            w_a = m_red*a.inverseMass;
            w_b = 1-w_a;
            
            % Compute center of mass
            com = p_a + offset*w_a;
            
             % Compute reduced inertia
             i_red = 1/(inverseSphericalInertiaA + inverseSphericalInertiaB);
             % Compute inertia weightings
             % = each body's proportion of any given angular impulse
             iw_a = i_red*inverseSphericalInertiaA;
             iw_b = 1-iw_a;
             
            % Calculate reduced moment of inertia of the whole system.
            totalLinearInertiaReciprocal = 1/(l*l*m_red);
            i_tot = 1/((1/i_red)+totalLinearInertiaReciprocal);
            
            % Compute motion weightings. In rotations where torque must be balanced by forces to maintain
            % total momentum, this is the share of the motion each type (angular/linear) must accept for
            % momentum to be conserved.
            mw_linear = i_tot*totalLinearInertiaReciprocal;
            mw_angular = 1-mw_linear;
                         
            %%%%%%%%% Solve first part of C_curve %%%%%%%%%%%%%%%%%%%%%%%%%%
            offset = p_b - p_a;
            l = norm(offset);
            oHat = ns(offset);
            t_aTilde = ns(project(t_a,oHat));
            t_bTilde = ns(project(t_b,oHat));
            
            % Signed angle between the projections
            crs = cross(t_aTilde,t_bTilde);
            sina = norm(crs);
            cosa = dot(t_aTilde,t_bTilde);
            sgn = dot(oHat,crs);
            ang = -abs(atan2(sina,cosa));
            
            if(cosa == 0)
                ang = 0;
            end
            ang = negateIfNegative(ang,sgn);
            absAng = abs(ang);
            sgnAng = ang*reciprocalAllValid(absAng);

            if(absAng > pi/2)
                ang = sgnAng*pi - ang;
            else
                ang = -ang;
            end
            
            ang_a = iw_a*ang;
            ang_b = iw_b*ang;
            
            t_a = ns(cheapRodrigues(t_a,ang_a*oHat));
            t_b = ns(cheapRodrigues(t_b,-ang_b*oHat));
            
            %%%%%%% Solve second part of C_curve %%%%%%%%%%%%%%%%%%%
            nHat = ns(cross(t_a,t_b));
            
            if(norm(nHat) == 0)
                nHat = ns(cross(t_a,oHat));
            end
            
            % Project the tangents into the plane normal and find the angle
            % between them and the offset.
            t_aDag = ns(project(t_a,nHat));
            t_bDag = ns(project(t_b,nHat));
            
            crs_a = cross(t_aDag,oHat);
            crs_b = cross(t_bDag,oHat);
            sina_a = norm(crs_a);
            sina_b = norm(crs_b);
            cosa_a = dot(t_aDag,oHat);
            cosa_b = dot(t_bDag,oHat);
            sgn_a = dot(nHat,crs_a);
            sgn_b = dot(nHat,crs_b);
            
            t_asa = -abs(atan2(sina_a,cosa_a));
            t_asa = zeroUnless(t_asa,cosa_a ~= 0);
            t_asa = negateIfNegative(t_asa,sgn_a);
            
            t_bsa = -abs(atan2(sina_b,cosa_b));
            t_bsa = zeroUnless(t_bsa,cosa_b ~= 0);
            t_bsa = negateIfNegative(t_bsa,sgn_b);
            
            deficitAngle = (t_asa + t_bsa);
            
            angular_angle = mw_angular*deficitAngle;
            linear_angle = 0.5*mw_linear*deficitAngle;
            
            oHat = ns(cheapRodrigues(oHat,linear_angle*nHat));
            
            comdA = l*w_a;
            comdB = l*w_b;
            
            p_a = com - comdA*oHat;
            p_b = com + comdB * oHat;
            
            ang_a = iw_a*angular_angle;
            ang_b = iw_b*angular_angle;
            
            t_a = ns(cheapRodrigues(t_a,-ang_a*nHat));
            t_b = ns(cheapRodrigues(t_b,-ang_b*nHat));
            

            %%%%%%%% Solve length constraint C_len. %%%%%%%%%%%%%%%%%%%%%%
            cosPsi = dot(t_a,oHat);
            sinPsi = norm(cross(t_a,oHat));
            dotSgn = cosPsi*reciprocalAllValid(abs(cosPsi));
            psi = atan2(sinPsi,cosPsi);

            targLen = constraint.l_0*sinPsi*reciprocalAllValid(psi);

            implsLen = l - targLen;
            p_a = p_a + w_a*implsLen*oHat;
            p_b = p_b - w_b*implsLen*oHat;
            
            %%%%%%%% Solve bending constraint C_bend. %%%%%%%%%%%%%%%%%%%%
            bendHardAngle = max(psi - constraint.bendHardLimit,0);
            bendSoftAngle = max(psi - constraint.bendSoftLimit,0);
                        
            totalFixAngle = -dotSgn*(bendHardAngle+constraint.alpha_b*bendSoftAngle);
            
            ang_a = totalFixAngle*iw_a;
            ang_b = totalFixAngle*iw_b;
            
            t_a = ns(cheapRodrigues(t_a,-ang_a*nHat));
            t_b = ns(cheapRodrigues(t_b,ang_b*nHat));
            
            % Find the euler vector accumulated thus far.
            t_a_evAx = ns(cross(t_a,initial_t_a));
            t_b_evAx = ns(cross(t_b,initial_t_b));
            t_a_eulerVector = -safeACos(dot(t_a,initial_t_a))*t_a_evAx;
            t_b_eulerVector = -safeACos(dot(t_b,initial_t_b))*t_b_evAx;
            
            %%%%%%%% Solve twisting constraint C_twist. %%%%%%%%%%%%%%%%%%%%
                        
            % Now we 'unbend' one of the normals so we can find what the
            % twist angle would be if there was no bending.
            qAx = cross(t_a,t_b);
            sina = norm(qAx);
            cosa = dot(t_a,t_b);
            qAx = ns(qAx);
            qAng = atan2(sina,cosa);
            
            % This is a finite rotation so we should use the full formula.
            n_a = ns(rodrigues(n_a,qAx*qAng));
            
            cosTwistAngle = dot(n_a,n_b);
            twistAxis = cross(n_a,n_b);
            
            tau = atan2(norm(twistAxis),cosTwistAngle);

            twistHardAngle =  max(tau - constraint.twistHardLimit,0);
            twistSoftAngle = max(tau - constraint.twistSoftLimit,0);
            
            twistTotalFixAngle = -(twistHardAngle+constraint.alpha_t*twistSoftAngle);
            qSgn = dot(t_a,twistAxis);
            twistTotalFixAngle = negateIfNegative(twistTotalFixAngle,qSgn);
            
            if( psi > pi/4)
                twistTotalFixAngle = -twistTotalFixAngle;
            end
            
            % Twisting angular impulses
            n_a_eulerVector = -twistTotalFixAngle*iw_a*t_a;
            n_b_eulerVector = twistTotalFixAngle*iw_b*t_b;

            % Find impulses required to bring the bodies to the solved
            % configuration.
                        
            a.angularDisplacement = a.angularDisplacement + t_a_eulerVector + n_a_eulerVector;
            b.angularDisplacement = b.angularDisplacement + t_b_eulerVector + n_b_eulerVector;
            a.displacement = a.displacement + (p_a-initial_p_a);
            b.displacement = b.displacement + (p_b-initial_p_b);

            aSolved = a;
            bSolved = b; 
        end
    end
    
end

