LocatorFromPointsSystem.h

Go to the documentation of this file.

/*  Copyright (C) 2003-2021 Free Electron Organization
    Any use of this software requires a license.  If a valid license
    was not distributed with this file, visit freeelectron.org. */

/** @file */

#ifndef __tire_LocatorFromPointsSystem_h__
#define __tire_LocatorFromPointsSystem_h__

#include "solve/solve.h"

namespace fe
{
namespace ext
{

class LocatorFromPointsSystem :
    virtual public Stepper,
    public Initialize<LocatorFromPointsSystem>
{
    public:
        struct Item
        {
            WeakRecord                  m_r_parent;
            WeakRecord                  m_r_p0;
            WeakRecord                  m_r_p1;
            WeakRecord                  m_r_p2;
            t_moa_xform                     m_inv_R;
            bool                            m_has_force;
        };
        LocatorFromPointsSystem(void) { }
        void    initialize(void) {}
virtual void    compile(const t_note_id &a_note_id)
        {
            sp<Scope> spScope = m_rg_dataset->scope();

            if(!spScope.isValid()) { return; }

            enforce<AsTire,AsTireLive>(m_rg_dataset);

            m_asPoint.bind(spScope);
            m_asForcePoint.bind(spScope);
            m_asLocatorFromPoints.bind(spScope);
            m_asForcePoints.bind(spScope);

            std::vector<Record> candidates;
            m_asLocatorFromPoints.filter(candidates, m_rg_dataset);

            for(unsigned int i_candidate = 0; i_candidate < candidates.size();
                i_candidate++)
            {
                Record &r_candidate = candidates[i_candidate];
                Item item;
                item.m_r_parent = r_candidate;
                item.m_r_p0 = m_asLocatorFromPoints.p0(r_candidate);
                item.m_r_p1 = m_asLocatorFromPoints.p1(r_candidate);
                item.m_r_p2 = m_asLocatorFromPoints.p2(r_candidate);
                if( item.m_r_p0.isValid() &&
                    item.m_r_p1.isValid() &&
                    item.m_r_p2.isValid() &&
                    m_asPoint.check(item.m_r_p0) &&
                    m_asPoint.check(item.m_r_p1) &&
                    m_asPoint.check(item.m_r_p2) )
                {

                    t_moa_xform frame;
                    t_moa_xform relative;
                    setIdentity(relative);

                    bool frame_made = makeFrame(frame,
                        m_asPoint.location(item.m_r_p0),
                        m_asPoint.location(item.m_r_p1),
                        m_asPoint.location(item.m_r_p2),
                        relative);

                    invert(item.m_inv_R, frame);

                    relative = m_asLocatorFromPoints.transform(r_candidate);

                    item.m_inv_R = relative * item.m_inv_R;

                    if(m_asForcePoints.check(r_candidate)
                        && m_asForcePoint.check(item.m_r_p0)
                        && m_asForcePoint.check(item.m_r_p1)
                        && m_asForcePoint.check(item.m_r_p2))
                    {
                        item.m_has_force = true;
                    }
                    else
                    {
                        item.m_has_force = false;
                    }

                    m_items.push_back(item);
                }
            }
        }
        void    step(t_moa_real a_dt)
        {
            for(unsigned int i_item = 0; i_item < m_items.size(); i_item++)
            {
                WeakRecord &r_p0 = m_items[i_item].m_r_p0;
                WeakRecord &r_p1 = m_items[i_item].m_r_p1;
                WeakRecord &r_p2 = m_items[i_item].m_r_p2;
                WeakRecord &r_item = m_items[i_item].m_r_parent;

                t_moa_xform frame;
                t_moa_xform relative;
                setIdentity(relative);
                bool frame_made = makeFrame(frame,
                    m_asPoint.location(r_p0),
                    m_asPoint.location(r_p1),
                    m_asPoint.location(r_p2),
                    m_items[i_item].m_inv_R);

                m_asLocatorFromPoints.transform(r_item) = frame;

                if(m_items[i_item].m_has_force)
                {
                    // p0 is the "reference root, so we'll use that velocity
                    t_moa_xform inv_frame;
                    invert(inv_frame, frame);
                    t_moa_v3 p0_v = rotateVector<3, t_moa_real>
                        (inv_frame, m_asForcePoint.velocity(r_p0));

                    m_asForcePoints.velocity(r_item) = p0_v;
                }
//TODO: ang vel -- frame ang is purely reflection -- useful ang vel from driveline
//TODO: grip point TM
            }

            //TODO: not sure accumulate should not be its own note
//the problem here is that step is "outbound" mutation, and accumulate
//is "inbound" mutation, so what happens out-there should happen in between, to optimize
//fidelity per step size
            accumulate();
        }
        void    accumulate(void)
        {
            for(unsigned int i_item = 0; i_item < m_items.size(); i_item++)
            {
                if(!m_items[i_item].m_has_force) { continue; }

                WeakRecord &r_p0 = m_items[i_item].m_r_p0;
                WeakRecord &r_p1 = m_items[i_item].m_r_p1;
                WeakRecord &r_p2 = m_items[i_item].m_r_p2;
                WeakRecord &r_item = m_items[i_item].m_r_parent;

                t_moa_xform xform = m_asLocatorFromPoints.transform(r_item);

                t_moa_v3 F = rotateVector<3, t_moa_real>
                    (xform, m_asForcePoints.force(r_item));
                t_moa_v3 M = rotateVector<3, t_moa_real>
                    (xform, m_asForcePoints.moment(r_item));

                t_moa_v3 P = xform.translation();

                applyFMToPoints(F, M, P, m_asForcePoint, r_p0, r_p1, r_p2);

#if 0
                t_moa_v3 A = m_asPoint.location(r_p0) - P;

                M[0] += - F[2] * A[1] + F[1] * A[2];
                M[1] +=   F[2] * A[0] - F[0] * A[2];
                M[2] += - F[1] * A[0] + F[0] * A[1];

                m_asForcePoint.force(r_p0) += F;

                t_moa_v3 p0 = m_asPoint.location(r_p0);
                t_moa_v3 p1 = m_asPoint.location(r_p1);
                t_moa_v3 p2 = m_asPoint.location(r_p2);

                t_moa_v3 F0(0.0,0.0,0.0);
                t_moa_v3 F1(0.0,0.0,0.0);
                t_moa_v3 F2(0.0,0.0,0.0);

                t_moa_xform frame;
                t_moa_xform relative;
                setIdentity(relative);
                bool frame_made = makeFrame(frame,
                    p0,
                    p1,
                    p2,
                    relative);

                t_moa_xform frame_inv;
                invert(frame_inv, frame);
                t_moa_v3 r0 = transformVector<3, t_moa_real>(frame_inv, p0);
                t_moa_v3 r1 = transformVector<3, t_moa_real>(frame_inv, p1);
                t_moa_v3 r2 = transformVector<3, t_moa_real>(frame_inv, p2);
                t_moa_v3 frameM = rotateVector<3, t_moa_real>(frame_inv,M);

                // M0
                t_moa_real arm = r2[2];
                F2[1] -= frameM[0] / arm;
                F0[1] += frameM[0] / arm;
                t_moa_real residue = (frameM[0] / arm) * r2[0];
                arm = r1[0];
                F1[1] += residue / arm;
                F0[1] -= residue / arm;

                // M1
                arm = r1[0];
                F1[2] -= frameM[1] / arm;
                F0[2] += frameM[1] / arm;

                // M2
                arm = r1[0];
                F1[1] += frameM[2] / arm;
                F0[1] -= frameM[2] / arm;

                F0 = rotateVector<3, t_moa_real>(frame, F0);
                F1 = rotateVector<3, t_moa_real>(frame, F1);
                F2 = rotateVector<3, t_moa_real>(frame, F2);

                m_asForcePoint.force(r_p0) += F0;
                m_asForcePoint.force(r_p1) += F1;
                m_asForcePoint.force(r_p2) += F2;
#endif
            }
        }

        std::vector<Item>       m_items;
        AsPoint     m_asPoint;
        AsForcePoint    m_asForcePoint;
        AsLocatorFromPoints     m_asLocatorFromPoints;
        AsForceLocator          m_asForcePoints;
};

} /* namespace ext */
} /* namespace fe */

#endif /* __tire_LocatorFromPointsSystem_h__ */