How to implement a constraint solver for 2-D geome

2020-01-25 02:38发布

站内问答 / JavaScript
830 1 5

I have a set of metallic sliding pieces which are constrained to the x and y axis in following way:

sliding pieces

I would need to maximize the horizontal distance among all pieces constrained by the same slider and the vertical distance among the sliding pieces and the sliders itself. How can this be solved?

Any advice and suggestion which can lead to a solution for this problem would be greatly appreciated.

I looked first at some very powerful libraries like cassowary and jsLPSolver but i had some trouble to understand the core algorithm and how the constraint are checked for feasibility and how the possible solutions are then ranked.

How could be implemented in JavaScript a (simple) stub for a 2-D geometric constraint solver for problems like this one above?

EDIT:

I have following input data:

maxW = 300, maxH = 320

The pieces are defined as follows (not mandatory, every solution is accepted):

slidingPiece = [pX, pY, width, height, anchorPoint, loopDistance];

I will try to explain what i mean under "maximize".

Horizontal spacing:

a0-b1, b1-b2, b2-b4, b4-b5 and b5-maxX will be the same, i.e. max X divided by the greatest number of vertical intersecting pieces + 1 (5). b1-b3 and b3-b5 will be then determined by the available remaining space.

Vertical spacing:

b1-a3, a3-a4 and a0-b5 will be the same. Ideally, a0-b3,b3-b4,a2-b2,b4-a3 and b2-a4 will be also the same value. Maximizing a1-b4 and b3-a2 is the same as maximizing b3-b4. The same applies to a2-b2 and b4-a3: the distance b2-b4 will be then the max negative value.

So, i need to maximize the distance among every sliding piece and his nearest above or below Y-constraint.

The 2-D geometric representation of this problem shows that the horizontal spacing depends from the vertical distance of the anchors (due to the vertical intersection of the anchored pieces), which in turn depends from the horizontal position of the pieces itself. Think for example, b2 is somewhat shorter above. In this case, b1 and b2 are no longer intersecting and would became the same x value, i.e. max X divided by 4.

In some other cases, for example b2 is much longer in the above part - and will cross the anchor a2, then it shall be spaced to a1. This is the reason because there will be a set of solutions, some feasible and some other not, because for example, the global max Y constraint would be broken.

1条回答
beautiful°
2楼-- · 2020-01-25 03:21

I would try field approach similar to this.

  1. Each slider will retract all sliders away

    with force scaled by distance^2 like all of them would have the same polarity electric charge or springs attached in between each other.

  2. On top of that add friction scaled by speed

    does not really matter if air v^2 or liquid v^3

  3. implement kinematic constraints

    for horizontal and vertical only sliding it should be really easy.

  4. Do physical simulation and wait until it converges to stable state v=~0

    if hit local min/max shake the whole thing a bit or arrange the whole thing randomly and try again. You can do this also to get another solution.

[Edit4] C++ solver example

  1. structures/classes to represent the slider system

    To ease up later code I will not support closed loops or double anchoring. That is why the i1 slider (most right) is not anchored to anything (will just provide forcefield). I ended up with this slider definition:

    slider def

    look at the source of class _slider for more info.

  2. render

    Dash-dash means fixed slider. Silver ones are horizontal, aqua means vertical and yellow is selected by mouse. May be later on red will mean some kind of error/stuck or something for debug purposes. For force field solvers I sometimes add the field strength as red-blue scale but not sure if I will implement it here or not.

    To keep this simple I will not implement zoom/pan functions as your dimensions are convenient for direct render without transforms.

    initial positions

  3. implement initial setup

    sliders sys;
int i0,i1,a0,a1,a2,a3,a4,b1,b2,b3,b4,b5;
sys.slider_beg();//ia,ib,   x,    y,    a0,    a1,    b0,    b1,_horizontal
i0=sys.slider_add(-1,-1, 25.0, 25.0,  -5.0, 405.0,   0.0,   0.0, 0);
a0=sys.slider_add(i0,-1,  0.0,  0.0,   0.0, 400.0,   0.0,   0.0, 1);
a1=sys.slider_add(i0,-1,  0.0,100.0,   0.0, 400.0,   0.0,   0.0, 1);
a2=sys.slider_add(i0,-1,  0.0,200.0,   0.0, 400.0,   0.0,   0.0, 1);
a3=sys.slider_add(i0,-1,  0.0,300.0,   0.0, 400.0,   0.0,   0.0, 1);
a4=sys.slider_add(i0,-1,  0.0,400.0,   0.0, 400.0,   0.0,   0.0, 1);
b1=sys.slider_add(a0,a2, 20.0,  0.0,   0.0, 125.0, 125.0, 250.0, 0);
b2=sys.slider_add(a3,-1, 40.0,  0.0, -70.0,  30.0,   0.0,   0.0, 0);
b3=sys.slider_add(a1,-1, 60.0,  0.0, -70.0,  30.0,   0.0,   0.0, 0);
b4=sys.slider_add(a2,-1, 80.0,  0.0, -30.0,  70.0,   0.0,   0.0, 0);
b5=sys.slider_add(a3,a1,100.0,  0.0,-125.0,   0.0,-125.0,-250.0, 0);
i1=sys.slider_add(-1,-1,425.0, 25.0,  -5.0, 405.0,   0.0,   0.0, 0);
sys.slider_end();

    Where ia is parent index and ib is child index (the slider class itself holds ib as parent but that would be confusing to init as you would need to link to item that do not exist yet so the ib transformation is handled in the sys.add function). sys is class holding the whole thing and sys.add just add new slider to it and returns its index counting from zero. The x,y is relative position to parent.

    To ease up amount of coding this setup must not conflict the constraints. The overview of this setup is in previous bullet.

    Beware the order of sliders must be left to right for vertical and top to bottom for horizontal sliders to ensure correct constraint functionality.

  4. mouse interaction

    just simple slider movement for debug and adjusting initial setup values. And or handling stuck cases. You need to handle mouse events, select closest slider if not editing already. And if mouse button is pressed move selected slider to mouse position...

  5. physical constraint/interaction

    I simplify this a bit so I just created a predicate function that is called for specified slider and it returns if it or any its child/anchor is in conflict with defined constraints. This is much more easy to code and debug then to update the position to match actual constraint.

    Usage is then a bit more code. First store actual position for updated slider. Then update slider to new position/state. After that if constraints are not met stop actual slider speeds and restore its original position.

    It will be a bit slower but I am too lazy to code the full constraint updater (that code could get really complex...).

    I recognize 2 interactions parallel and perpendicular. The parallel is straight forward. But the perpendicular is interaction between edge of slider and perpendicular sliders near it not including the already intersecting sliders (a,b anchored or just crossing) during initial state. So I created a list of intersecting sliders (ic) at start which will be ignored for this interaction.

  6. physical simulation

    Simple Newton - D'Alembert physics for non relativistic speeds will do. Just on each iteration set the accelerations ax,ay to the field strength and frictions.

  7. field solver

    This is set of rules/equations to set simulation accelerations for each slider to converge to solution. I ended up with electrostatic retracting force F = -Q/r^2 and linear dampening of speed. Also have implemented absolute velocity and acceleration limiters to avoid numeric problems.

    To boost solution time and stability I added precision control modes where the electric charge is lowering when overall max speed of sliders is decreasing.

Here The full C++/VCL class code for this:

//---------------------------------------------------------------------------
//--- Sliders solver ver: 1.01 ----------------------------------------------
//---------------------------------------------------------------------------
#ifndef _sliders_h
#define _sliders_h
//---------------------------------------------------------------------------
#include <math.h>
#include "list.h"   // linear dynamic array template List<T> similar to std::vector
//---------------------------------------------------------------------------
const double _slider_w   =   3.00;  // [px] slider half width (for rendering)
const double _slider_gap =   4.00;  // [px] min gap between sliders (for colisions)
const double _acc_limit=   100.00;  // [px/s^2]
const double _vel_limit=   100.00;  // [px/s]
const double _friction =     0.90;  // [-]
const double _charge   =250000.00;  // [px^3/s^2]
//---------------------------------------------------------------------------
class _slider   // one slider (helper class)
    {
public:
    // properties
    double x,y;             // actual relative pos
    bool _horizontal;       // orientation
    double a0,a1;           // slider vertexes 0 is anchor point
    double b0,b1;           // anchor zone for another slider
    int ia;                 // -1 for fixed or index of parrent slider
    int ib;                 // -1 or index of parrent slider
    // computed
    List<int> ic;           // list of slider indexes to ignore for perpendicular constraints
    double a,b;             // force field affected part
    double X,Y;             // actual absolute position
    double vx,vy,ax,ay;     // actual relative vel,acc
    // temp
    int flag;               // temp flag for simulation
    double x0,x1;           // temp variables for solver
    // constructors (can ignore this)
    _slider()           {}
    _slider(_slider& a) { *this=a; }
    ~_slider()          {}
    _slider* operator = (const _slider *a) { *this=*a; return this; }
    //_slider* operator = (const _slider &a) { ...copy... return this; }
    };
//---------------------------------------------------------------------------
class sliders   // whole slider system main class
    {
public:
    List<_slider> slider;           // list of sliders

    double vel_max;                 // max abs velocity of sliders for solver precision control
    double charge;                  // actual charge of sliders for solve()
    int    mode;                    // actual solution precision control mode

    // constructors (can ignore this)
    sliders();
    sliders(sliders& a) { *this=a; }
    ~sliders()          {}
    sliders* operator = (const sliders *a) { *this=*a; return this; }
    //sliders* operator = (const sliders &a) { ...copy... return this; }

    // VCL window API variables (can ignore this)
    double mx0,my0,mx1,my1; // last and actual mouse position
    TShiftState sh0,sh1;    // last and actual mouse buttons and control keys state
    int sel;

    // API (this is important stuff)
    void slider_beg(){ slider.num=0; }  // clear slider list
    int  slider_add(int ia,int ib,double x,double y,double a0,double a1,double b0,double b1,bool _h); // add slider to list
    void slider_end();              // compute slider parameters
    bool constraints(int ix);       // return true if constraints hit
    void positions();               // recompute absolute positions
    void update(double dt);         // update physics simulation with time step dt [sec]
    void solve(bool _init=false);   // set sliders accelerations to solve this
    void stop();                    // stop all movements
    // VCL window API for interaction with GUI (can ignore this)
    void mouse(int x,int y,TShiftState sh);
    void draw(TCanvas *scr);
    };
//---------------------------------------------------------------------------
sliders::sliders()
    {
    mx0=0.0; my0=0.0;
    mx1=0.0; my1=0.0;
    sel=-1;
    }
//---------------------------------------------------------------------------
int sliders::slider_add(int ia,int ib,double x,double y,double a0,double a1,double b0,double b1,bool _h)
    {
    _slider s; double q;
    if (a0>a1) { q=a0; a0=a1; a1=q; }
    if (b0>b1) { q=b0; b0=b1; b1=q; }
    s.x=x; s.vx=0.0; s.ax=0.0;
    s.y=y; s.vy=0.0; s.ay=0.0;
    s.ia=ia; s.a0=a0; s.a1=a1;
    s.ib=-1; s.b0=b0; s.b1=b1;
    s.ic.num=0;
    if ((ib>=0)&&(ib<slider.num)) slider[ib].ib=slider.num;
    s._horizontal=_h;
    s.a=a0; // min
    if (s.a>a1) s.a=a1;
    if (s.a>b0) s.a=b0;
    if (s.a>b1) s.a=b1;
    s.b=a0; // max
    if (s.b<a1) s.b=a1;
    if (s.b<b0) s.b=b0;
    if (s.b<b1) s.b=b1;
    slider.add(s);
    return slider.num-1;
    }
//---------------------------------------------------------------------------
void sliders::slider_end()
    {
    int i,j;
    double a0,a1,b0,b1,x0,x1,w=_slider_gap;
    _slider *si,*sj;
    positions();
    // detect intersecting sliders and add them to propriet ic ignore list
    for (si=slider.dat,i=0;i<slider.num;i++,si++)
     for (sj=si+1   ,j=i+1;j<slider.num;j++,sj++)
      if (si->_horizontal!=sj->_horizontal)
        {
        if (si->_horizontal)
            {
            a0=si->X+si->a; a1=sj->X-w;
            b0=si->X+si->b; b1=sj->X+w;
            x0=si->Y;       x1=sj->Y;
            }
        else{
            a0=si->Y+si->a; a1=sj->Y-w;
            b0=si->Y+si->b; b1=sj->Y+w;
            x0=si->X;       x1=sj->X;
            }
        if (((a0<=b1)&&(b0>=a1))||((a1<=b0)&&(b1>=a0)))
         if ((x0>x1+sj->a-w)&&(x0<x1+sj->b+w))
            {
            si->ic.add(j);
            sj->ic.add(i);
            }
        }
    }
//---------------------------------------------------------------------------
bool sliders::constraints(int ix)
    {
    int i,j;
    double a0,a1,b0,b1,x0,x1,x,w=_slider_gap;
    _slider *si,*sj,*sa,*sb,*s;
    s=slider.dat+ix;
    // check parallel neighbors overlapp
    for (si=slider.dat,i=0;i<slider.num;i++,si++)
     if ((i!=ix)&&(si->_horizontal==s->_horizontal))
        {
        if (s->_horizontal)
            {
            a0=s->X+s->a; a1=si->X+si->a;
            b0=s->X+s->b; b1=si->X+si->b;
            x0=s->Y;      x1=si->Y;
            }
        else{
            a0=s->Y+s->a; a1=si->Y+si->a;
            b0=s->Y+s->b; b1=si->Y+si->b;
            x0=s->X;      x1=si->X;
            }
        if (((a0<=b1)&&(b0>=a1))||((a1<=b0)&&(b1>=a0)))
            {
            if ((i<ix)&&(x0<x1+w)) return true;
            if ((i>ix)&&(x0>x1-w)) return true;
            }
        }
    // check perpendicular neighbors overlapp
    for (si=slider.dat,i=0;i<slider.num;i++,si++)
     if ((i!=ix)&&(si->_horizontal!=s->_horizontal))
        {
        // skip ignored sliders for this
        for (j=0;j<s->ic.num;j++)
         if (s->ic[j]==i) { j=-1; break; }
          if (j<0) continue;
        if (s->_horizontal)
            {
            a0=s->X+s->a; a1=si->X-w;
            b0=s->X+s->b; b1=si->X+w;
            x0=s->Y;      x1=si->Y;
            }
        else{
            a0=s->Y+s->a; a1=si->Y-w;
            b0=s->Y+s->b; b1=si->Y+w;
            x0=s->X;      x1=si->X;
            }
        if (((a0<=b1)&&(b0>=a1))||((a1<=b0)&&(b1>=a0)))
         if ((x0>x1+si->a-w)&&(x0<x1+si->b+w))
          return true;
        }
    // conflict a anchor area of parent?
    if (s->ia>=0)
        {
        si=slider.dat+s->ia;
        if (s->_horizontal)
            {
            x0=si->Y+si->a0;
            x1=si->Y+si->a1;
            x=s->Y;
            }
        else{
            x0=si->X+si->a0;
            x1=si->X+si->a1;
            x=s->X;
            }
        if (x<x0+w) return true;
        if (x>x1-w) return true;
        }
    // conflict b anchor area of parent?
    if (s->ib>=0)
        {
        si=slider.dat+s->ib;
        if (si->_horizontal)
            {
            x0=si->X+si->b0;
            x1=si->X+si->b1;
            x=s->X;
            }
        else{
            x0=si->Y+si->b0;
            x1=si->Y+si->b1;
            x=s->Y;
            }
        if (x<x0+w) return true;
        if (x>x1-w) return true;
        }
    // conflict b anchor area with childs?
    for (si=slider.dat,i=0;i<slider.num;i++,si++)
     if ((i!=ix)&&(si->ib==ix))
        {
        if (s->_horizontal)
            {
            x0=s->X+s->b0;
            x1=s->X+s->b1;
            x=si->X;
            }
        else{
            x0=s->Y+s->b0;
            x1=s->Y+s->b1;
            x=si->Y;
            }
        if (x<x0+w) return true;
        if (x>x1-w) return true;
        }

    // check childs too
    for (si=slider.dat,i=0;i<slider.num;i++,si++)
     if ((i!=ix)&&(si->ia==ix))
      if (constraints(i)) return true;
    return false;
    }
//---------------------------------------------------------------------------
void sliders::positions()
    {
    int i,e;
    _slider *si,*sa;
    // set flag = uncomputed
    for (si=slider.dat,i=0;i<slider.num;i++,si++) si->flag=0;
    // iterate until all sliders are computed
    for (e=1;e;)
     for (e=0,si=slider.dat,i=0;i<slider.num;i++,si++)
      if (!si->flag)
        {
        // fixed
        if (si->ia<0)
            {
            si->X=si->x;
            si->Y=si->y;
            si->flag=1;
            continue;
            }
        // a anchored
        sa=slider.dat+si->ia;
        if (sa->flag)
            {
            si->X=sa->X+si->x;
            si->Y=sa->Y+si->y;
            si->flag=1;
            continue;
            }
        e=1; // not finished yet
        }
    }
//---------------------------------------------------------------------------
void sliders::update(double dt)
    {
    int i;
    _slider *si,*sa;
    double x,X;
    // D'Lamnbert integration
    for (si=slider.dat,i=0;i<slider.num;i++,si++)
     if (si->_horizontal)
        {
        x=si->y; si->vy+=si->ay*dt;     // vel = Integral(acc*dt)
                 si->vy*=_friction;     // friction k*vel
        X=si->Y; si->y +=si->vy*dt;     // pos = Integral(vel*dt)
        positions();                    // recompute childs
        if ((si->ia<0)||(constraints(i))) // if fixed or constraint hit (stop and restore original position)
            {
            si->vy=0.0;
            si->y =x;
            si->Y =X;
            positions();                // recompute childs
            }
        }
    else{
        x=si->x; si->vx+=si->ax*dt;     // vel = Integral(acc*dt)
                 si->vx*=_friction;     // friction k*vel
        X=si->X; si->x +=si->vx*dt;     // pos = Integral(vel*dt)
        positions();                    // recompute childs
        if ((si->ia<0)||(constraints(i))) // if fixed or constraint hit (stop and restore original position)
            {
            si->vx=0.0;
            si->x =x;
            si->X =X;
            positions();                // recompute childs
            }
        }
    }
//---------------------------------------------------------------------------
void sliders::solve(bool _init)
    {
    int i,j,k;
    double a0,a1,b0,b1,x0,x1;
    _slider *si,*sj,*sa;
    // init solution
    if (_init)
        {
        mode=0;
        charge=_charge;
        }
    // clear accelerations and compute actual max velocity
    vel_max=0.0;
    for (si=slider.dat,i=0;i<slider.num;i++,si++)
        {
        si->ax=0.0;
        si->ay=0.0;
        x0=fabs(si->vx); if (vel_max<x0) vel_max=x0;
        x0=fabs(si->vy); if (vel_max<x0) vel_max=x0;
        }
    // precision control of solver
    if ((mode==0)&&(vel_max>25.0)) { mode++; }                  // wait until speed raises
    if ((mode==1)&&(vel_max<10.0)) { mode++; charge*=0.10; }    // scale down forces to lower jitter
    if ((mode==2)&&(vel_max< 1.0)) { mode++; charge*=0.10; }    // scale down forces to lower jitter
    if ((mode==3)&&(vel_max< 0.1)) { mode++; charge =0.00; stop(); } // solution found
    // set x0 as 1D vector to closest parallel neighbor before and x1 after
    for (si=slider.dat,i=0;i<slider.num;i++,si++) { si->x0=0.0; si->x1=0.0; }
    for (si=slider.dat,i=0;i<slider.num;i++,si++)
     for (sj=si+1   ,j=i+1;j<slider.num;j++,sj++)
      if (si->_horizontal==sj->_horizontal)
        {
        // longer side interaction
        if (si->_horizontal)
            {
            a0=si->X+si->a; a1=sj->X+sj->a;
            b0=si->X+si->b; b1=sj->X+sj->b;
            x0=si->Y;       x1=sj->Y;
            }
        else{
            a0=si->Y+si->a; a1=sj->Y+sj->a;
            b0=si->Y+si->b; b1=sj->Y+sj->b;
            x0=si->X;       x1=sj->X;
            }
        if (((a0<=b1)&&(b0>=a1))||((a1<=b0)&&(b1>=a0)))
            {
            x0=x1-x0;
            if ((si->ia>=0)&&(x0<0.0)&&((fabs(si->x0)<_slider_gap)||(fabs(si->x0)>fabs(x0)))) si->x0=-x0;
            if ((si->ia>=0)&&(x0>0.0)&&((fabs(si->x1)<_slider_gap)||(fabs(si->x1)>fabs(x0)))) si->x1=-x0;
            if ((sj->ia>=0)&&(x0<0.0)&&((fabs(sj->x0)<_slider_gap)||(fabs(sj->x0)>fabs(x0)))) sj->x0=+x0;
            if ((sj->ia>=0)&&(x0>0.0)&&((fabs(sj->x1)<_slider_gap)||(fabs(sj->x1)>fabs(x0)))) sj->x1=+x0;
            }
        // shorter side interaction
        if (si->_horizontal)
            {
            a0=si->Y-_slider_gap; a1=sj->Y+_slider_gap;
            b0=si->Y+_slider_gap; b1=sj->Y+_slider_gap;
            x0=si->X;             x1=sj->X;
            }
        else{
            a0=si->X-_slider_gap; a1=sj->X+_slider_gap;
            b0=si->X+_slider_gap; b1=sj->X+_slider_gap;
            x0=si->Y;             x1=sj->Y;
            }
        if (((a0<=b1)&&(b0>=a1))||((a1<=b0)&&(b1>=a0)))
            {
            if (x0<x1) { x0+=si->b; x1+=sj->a; }
            else       { x0+=si->a; x1+=sj->b; }
            x0=x1-x0;
            if (si->ia>=0)
                {
                sa=slider.dat+si->ia;
                if ((sa->ia>=0)&&(x0<0.0)&&((fabs(sa->x0)<_slider_gap)||(fabs(sa->x0)>fabs(x0)))) sa->x0=-x0;
                if ((sa->ia>=0)&&(x0>0.0)&&((fabs(sa->x1)<_slider_gap)||(fabs(sa->x1)>fabs(x0)))) sa->x1=-x0;
                }
            if (sj->ia>=0)
                {
                sa=slider.dat+sj->ia;
                if ((sa->ia>=0)&&(x0<0.0)&&((fabs(sa->x0)<_slider_gap)||(fabs(sa->x0)>fabs(x0)))) sa->x0=+x0;
                if ((sa->ia>=0)&&(x0>0.0)&&((fabs(sa->x1)<_slider_gap)||(fabs(sa->x1)>fabs(x0)))) sa->x1=+x0;
                }
            }
        }
    // set x0 as 1D vector to closest perpendicular neighbor before and x1 after
    for (si=slider.dat,i=0;i<slider.num;i++,si++)
     for (sj=si+1   ,j=i+1;j<slider.num;j++,sj++)
      if (si->_horizontal!=sj->_horizontal)
        {
        // skip ignored sliders for this
        for (k=0;k<si->ic.num;k++)
         if (si->ic[k]==j) { k=-1; break; }
          if (k<0) continue;
        if (si->_horizontal)
            {
            a0=si->X+si->a; a1=sj->X-_slider_w;
            b0=si->X+si->b; b1=sj->X+_slider_w;
            x0=si->Y;
            }
        else{
            a0=si->Y+si->a; a1=sj->Y-_slider_w;
            b0=si->Y+si->b; b1=sj->Y+_slider_w;
            x0=si->X;
            }
        if (((a0<=b1)&&(b0>=a1))||((a1<=b0)&&(b1>=a0)))
            {
            if (si->_horizontal)
                {
                a1=sj->Y+sj->a;
                b1=sj->Y+sj->b;
                }
            else{
                a1=sj->X+sj->a;
                b1=sj->X+sj->b;
                }
            a1-=x0; b1-=x0;
            if (fabs(a1)<fabs(b1)) x0=-a1; else x0=-b1;
            if ((si->ia>=0)&&(x0<0.0)&&((fabs(si->x0)<_slider_gap)||(fabs(si->x0)>fabs(x0)))) si->x0=+x0;
            if ((si->ia>=0)&&(x0>0.0)&&((fabs(si->x1)<_slider_gap)||(fabs(si->x1)>fabs(x0)))) si->x1=+x0;
            if (sj->ia<0) continue;
            sa=slider.dat+sj->ia;
            if ((sa->ia>=0)&&(x0<0.0)&&((fabs(sa->x0)<_slider_gap)||(fabs(sa->x0)>fabs(x0)))) sa->x0=-x0;
            if ((sa->ia>=0)&&(x0>0.0)&&((fabs(sa->x1)<_slider_gap)||(fabs(sa->x1)>fabs(x0)))) sa->x1=-x0;
            }
        }
    // convert x0,x1 distances to acceleration
    for (si=slider.dat,i=0;i<slider.num;i++,si++)
        {
        // driving force F = ~ Q / r^2
        if (fabs(si->x0)>1e-10)  x0=charge/(si->x0*si->x0); else x0=0.0; if (si->x0<0.0) x0=-x0;
        if (fabs(si->x1)>1e-10)  x1=charge/(si->x1*si->x1); else x1=0.0; if (si->x1<0.0) x1=-x1;
        a0=x0+x1;
        // limit acc
        if (a0<-_acc_limit) a0=-_acc_limit;
        if (a0>+_acc_limit) a0=+_acc_limit;
        // store parallel acc to correct axis
        if (si->_horizontal) si->ay=a0;
         else                si->ax=a0;
        // limit vel (+/- one iteration overlap)
        if (si->_horizontal) x0=si->vy;
         else                x0=si->vx;
        if (x0<-_vel_limit)  x0=-_vel_limit;
        if (x0>+_vel_limit)  x0=+_vel_limit;
        if (si->_horizontal) si->vy=x0;
         else                si->vx=x0;
        }
    }
//---------------------------------------------------------------------------
void sliders::stop()
    {
    int i;
    _slider *si;
    for (si=slider.dat,i=0;i<slider.num;i++,si++)
        {
        si->vx=0.0;
        si->vy=0.0;
        si->ax=0.0;
        si->ay=0.0;
        }
    }
//---------------------------------------------------------------------------
void sliders::mouse(int x,int y,TShiftState sh)
    {
    int i,q0,q1;
    double d,dd;
    _slider *si;
    // update mouse state
    mx0=mx1; my0=my1; sh0=sh1;
    mx1=x;   my1=y;   sh1=sh;
    // slider movement with left mouse button
    q0=sh0.Contains(ssLeft);
    q1=sh1.Contains(ssLeft);
    if ((sel>=0)&&(q1))
        {
        si=slider.dat+sel;
        // stop simulation for selected slider
        si->vx=0.0;
        si->vy=0.0;
        si->ax=0.0;
        si->ay=0.0;
        // use mouse position instead
        if (si->ia>=0)
            {
            if (si->_horizontal){ d=si->y; dd=si->Y; si->y+=my1-si->Y; si->Y=my1; si->vy=0.0; si->ay=0.0; positions(); if (constraints(sel)) { si->y=d; si->Y=dd; positions(); }}
             else               { d=si->x; dd=si->X; si->x+=mx1-si->X; si->X=mx1; si->vx=0.0; si->ax=0.0; positions(); if (constraints(sel)) { si->x=d; si->X=dd; positions(); }}
            }
        }
    // select slider (if not left mouse button used)
    if (!q1)
     for (sel=-1,d=_slider_w+1.0,si=slider.dat,i=0;i<slider.num;i++,si++)
        {
        dd=_slider_w+1.0;
        if (si->_horizontal){ if ((mx1>=si->X+si->a)&&(mx1<=si->X+si->b)) dd=fabs(my1-si->Y); }
         else               { if ((my1>=si->Y+si->a)&&(my1<=si->Y+si->b)) dd=fabs(mx1-si->X); }
        if ((dd<d)&&(dd<=_slider_w)) { sel=i; d=dd; }
        }
    }
//---------------------------------------------------------------------------
void sliders::draw(TCanvas *scr)
    {
    int i,j,n;
    double w=_slider_w,r,x,y,a0,a1;
    AnsiString txt;
    _slider *s;
    scr->Brush->Style=bsClear;
    #define _line(aa,bb)           \
    if (s->_horizontal)            \
        {                          \
        scr->MoveTo(s->X+aa,s->Y); \
        scr->LineTo(s->X+bb,s->Y); \
        }                          \
    else{                          \
        scr->MoveTo(s->X,s->Y+aa); \
        scr->LineTo(s->X,s->Y+bb); \
        }
    scr->Pen->Color=clSilver;
    scr->Font->Color=clWhite;
    scr->TextOutA(40,40,AnsiString().sprintf("mode %i",mode));
    scr->TextOutA(40,60,AnsiString().sprintf("vel: %.3lf [px/s]",vel_max));
    scr->TextOutA(40,80,AnsiString().sprintf("  Q: %.3lf [px^3/s^2]",charge));
    scr->Font->Color=clYellow;
    for (s=slider.dat,i=0;i<slider.num;i++,s++)
        {
        if (s->_horizontal) scr->Pen->Color=clSilver;
         else               scr->Pen->Color=clAqua;
        if (i==sel)
            {
            scr->Pen->Color=clYellow;
            txt=AnsiString().sprintf(" ix:%i ia:%i ib:%i ic:",sel,s->ia,s->ib);
            for (j=0;j<=s->ic.num;j++) txt+=AnsiString().sprintf(" %i",s->ic[j]);
            scr->TextOutA(40,100,txt);
            scr->TextOutA(40,120,AnsiString().sprintf("pos: %.1lf %.1lf [px]",s->X,s->Y));
            scr->TextOutA(40,140,AnsiString().sprintf("vel: %.3lf %.3lf [px/s]",s->vx,s->vy));
            scr->TextOutA(40,160,AnsiString().sprintf("acc: %.3lf %.3lf [px/s^2]",s->ax,s->ay));
            scr->Pen->Color=clYellow;
            }
        if (s->ia<0) scr->Pen->Style=psDash;
         else        scr->Pen->Style=psSolid;
        // a anchor loop
        x=s->X;
        y=s->Y;
        if (s->ia>=0) scr->Ellipse(x-w,y-w,x+w,y+w);
        // b anchor loop
        r=0.5*fabs(s->b1-s->b0);
        if (s->_horizontal)
            {
            x=s->X+0.5*(s->b0+s->b1);
            y=s->Y;
            scr->RoundRect(x-r,y-w,x+r,y+w,w,w);
            }
        else{
            x=s->X;
            y=s->Y+0.5*(s->b0+s->b1);
            scr->RoundRect(x-w,y-r,x+w,y+r,w,w);
            }
        // a line cutted by a anchor loop
        a0=s->a0; a1=s->a1;
        if ((s->ia>=0)&&(a0<=+w)&&(a1>=-w))
            {
            if (a0<-w) _line(s->a0,-w);
            if (a1>+w) _line( w,s->a1);
            }
        else _line(s->a0,s->a1);
        }
    scr->Font->Color=clDkGray;
    scr->Pen->Style=psSolid;
    scr->Brush->Style=bsSolid;
    #undef _line
    }
//---------------------------------------------------------------------------
#endif
//---------------------------------------------------------------------------

You can ignore the VCL stuff it is just API for interaction with my App window and rendering. The solver itself does not need anything from it. I used my dynamic linear array template List<T> so here few explanations:

  • List<double> xxx; is the same as double xxx[];
  • xxx.add(5); adds 5 to end of the list
  • xxx[7] access array element (safe)
  • xxx.dat[7] access array element (unsafe but fast direct access)
  • xxx.num is the actual used size of the array
  • xxx.reset() clears the array and set xxx.num=0
  • xxx.allocate(100) preallocate space for 100 items

Usage is simple after proper init from bullet #3 like this:

sys.solve(true);
for (;;)
 {
 sys.solve();
 sys.update(0.040); // just time step
 if (sys.mode==4) break; // stop if solution found or stuck
 }

Instead of for cycle I call this in timer and redraw the window so I see the animation:

animation

The choppyness is due to non uniform GIF grabbing sample rate (skipping some frames from the simulation irregularly).

You can play with the constants for vel,acc limits, dampening coefficient and the mode control ifs to change the behavior. If you implement also mouse handler then you can move the sliders with left mouse button so you can get out of stuck cases...

Here stand alone Win32 demo (compiled with BDS2006 C++).

  • Demo click on slow download below the big magenta button, enter 4 letter alphanumeric code to start download no registration needed.

For more info about how the solver Force computation works see related/followup QA:

查看更多
0人赞 添加讨论(0) 举报
登录 后发表回答
相关问题
查看全部
相关文章
查看全部
收藏的人(5)