rect_constraint.cpp

Go to the documentation of this file.

/****************************************
 * INCLUDES 
 ****************************************/

#include "rect_constraint.h"

/****************************************
 * FUNCTIONS 
 ****************************************/

RectangularConstraint_ZMP::RectangularConstraint_ZMP(const double *d_) : vert(4,2)
{
    D[0] =  1.0; D[4] =  0.0;
    D[1] =  0.0; D[5] =  1.0;
    D[2] = -1.0; D[6] =  0.0;
    D[3] =  0.0; D[7] = -1.0;

    d[0] = d_[0]; 
    d[1] = d_[1]; 
    d[2] = d_[2]; 
    d[3] = d_[3];

    d_orig[0] = d[0];
    d_orig[1] = d[1];
    d_orig[2] = d[2];
    d_orig[3] = d[3];

    Constraints2Vert(); // determine coordinates of vertices
}



void RectangularConstraint_ZMP::rotate_translate(const double ca, const double sa, const double x, const double y)
{
    // D = D*R'
    D[0] =  ca; D[4] =  sa;
    D[1] = -sa; D[5] =  ca;
    D[2] = -ca; D[6] = -sa;
    D[3] =  sa; D[7] = -ca;

    // d = d - D*p
    d[0] = d_orig[0] + D[0]*x + D[4]*y;
    d[1] = d_orig[1] + D[1]*x + D[5]*y;
    d[2] = d_orig[2] + D[2]*x + D[6]*y;
    d[3] = d_orig[3] + D[3]*x + D[7]*y;

    Constraints2Vert(); // determine coordinates of vertices
}



void RectangularConstraint_ZMP::Constraints2Vert()
{
    double det;
    
    // Indexes of ctr.D
    // 0 4
    // 1 5 
    // 2 6
    // 3 7

    //   D     d       inv (D)
    // |0 4|   0    1/det * | 7 -4|
    // |3 7|   3            |-3  0|
    det = D[0]*D[7] - D[3]*D[4];
    vert(0,0) =  D[7]/det*d[0] - D[4]/det*d[3];
    vert(0,1) = -D[3]/det*d[0] + D[0]/det*d[3]; 
    
    // |0 4|   0     | 5 -4|
    // |1 5|   1     |-1  0|
    det = D[0]*D[5] - D[4]*D[1]; 
    vert(1,0) =  D[5]/det*d[0] - D[4]/det*d[1];
    vert(1,1) = -D[1]/det*d[0] + D[0]/det*d[1]; 
    
    // |1 5|   1     | 6 -5|
    // |2 6|   2     |-2  1|
    det = D[1]*D[6] - D[5]*D[2]; 
    vert(2,0) =  D[6]/det*d[1] - D[5]/det*d[2];
    vert(2,1) = -D[2]/det*d[1] + D[1]/det*d[2]; 
    
    // |2 6|   2     | 7 -6|
    // |3 7|   3     |-3  2|
    det = D[2]*D[7] - D[3]*D[6]; 
    vert(3,0) =  D[7]/det*d[2] - D[6]/det*d[3];
    vert(3,1) = -D[3]/det*d[2] + D[2]/det*d[3]; 
}