static unsigned int _shape_add(Entity ent, PhysicsShape type, cpShape *shape)
{
PhysicsInfo *info;
ShapeInfo *shapeInfo;
info = entitypool_get(pool, ent);
error_assert(info);
/* init ShapeInfo */
shapeInfo = array_add(info->shapes);
shapeInfo->type = type;
shapeInfo->shape = shape;
/* init cpShape */
cpShapeSetBody(shape, info->body);
cpSpaceAddShape(space, shape);
cpShapeSetFriction(shapeInfo->shape, 1);
cpShapeSetUserData(shapeInfo->shape, ent);
/* update moment */
if (!cpBodyIsStatic(info->body))
{
if (array_length(info->shapes) > 1)
cpBodySetMoment(info->body, _moment(info->body, shapeInfo)
+ cpBodyGetMoment(info->body));
else
cpBodySetMoment(info->body, _moment(info->body, shapeInfo));
}
return array_length(info->shapes) - 1;
}
/* recalculate moment for whole body by adding up shape moments */
static void _recalculate_moment(PhysicsInfo *info)
{
Scalar moment;
ShapeInfo *shapeInfo;
if (!info->body)
return;
if (array_length(info->shapes) == 0)
return; /* can't set moment to zero, just leave it alone */
moment = 0.0;
array_foreach(shapeInfo, info->shapes)
moment += _moment(info->body, shapeInfo);
cpBodySetMoment(info->body, moment);
}
void Kinematics::_quadrilateralInverse(float xTarget,float yTarget, float* aChainLength, float* bChainLength){
//coordinate shift to put (0,0) in the center of the plywood from the left sprocket
y = (halfHeight) + sysSettings.motorOffsetY - yTarget;
x = (sysSettings.distBetweenMotors/2.0) + xTarget;
//Coordinates definition:
// x -->, y |
// v
// (0,0) at center of left sprocket
// upper left corner of plywood (270, 270)
byte Tries = 0; //initialize
if(x > sysSettings.distBetweenMotors/2.0){ //the right half of the board mirrors the left half so all computations are done using left half coordinates.
x = sysSettings.distBetweenMotors-x; //Chain lengths are swapped at exit if the x,y is on the right half
Mirror = true;
}
else{
Mirror = false;
}
TanGamma = y/x;
TanLambda = y/(sysSettings.distBetweenMotors-x);
Y1Plus = R * sqrt(1 + TanGamma * TanGamma);
Y2Plus = R * sqrt(1 + TanLambda * TanLambda);
while (Tries <= KINEMATICSMAXINVERSE) {
_MyTrig();
//These criteria will be zero when the correct values are reached
//They are negated here as a numerical efficiency expedient
Crit[0]= - _moment(Y1Plus, Y2Plus, MySinPhi, SinPsi1, CosPsi1, SinPsi2, CosPsi2);
Crit[1] = - _YOffsetEqn(Y1Plus, x - h * CosPsi1, SinPsi1);
Crit[2] = - _YOffsetEqn(Y2Plus, sysSettings.distBetweenMotors - (x + h * CosPsi2), SinPsi2);
if (abs(Crit[0]) < KINEMATICSMAXERROR) {
if (abs(Crit[1]) < KINEMATICSMAXERROR) {
if (abs(Crit[2]) < KINEMATICSMAXERROR){
break;
}
}
}
//estimate the tilt angle that results in zero net _moment about the pen
//and refine the estimate until the error is acceptable or time runs out
//Estimate the Jacobian components
Jac[0] = (_moment( Y1Plus, Y2Plus, MySinPhiDelta, SinPsi1D, CosPsi1D, SinPsi2D, CosPsi2D) + Crit[0])/DELTAPHI;
Jac[1] = (_moment( Y1Plus + DELTAY, Y2Plus, MySinPhi, SinPsi1, CosPsi1, SinPsi2, CosPsi2) + Crit[0])/DELTAY;
Jac[2] = (_moment(Y1Plus, Y2Plus + DELTAY, MySinPhi, SinPsi1, CosPsi1, SinPsi2, CosPsi2) + Crit[0])/DELTAY;
Jac[3] = (_YOffsetEqn(Y1Plus, x - h * CosPsi1D, SinPsi1D) + Crit[1])/DELTAPHI;
Jac[4] = (_YOffsetEqn(Y1Plus + DELTAY, x - h * CosPsi1,SinPsi1) + Crit[1])/DELTAY;
Jac[5] = 0.0;
Jac[6] = (_YOffsetEqn(Y2Plus, sysSettings.distBetweenMotors - (x + h * CosPsi2D), SinPsi2D) + Crit[2])/DELTAPHI;
Jac[7] = 0.0;
Jac[8] = (_YOffsetEqn(Y2Plus + DELTAY, sysSettings.distBetweenMotors - (x + h * CosPsi2D), SinPsi2) + Crit[2])/DELTAY;
//solve for the next guess
_MatSolv(); // solves the matrix equation Jx=-Criterion
// update the variables with the new estimate
Phi = Phi + Solution[0];
Y1Plus = Y1Plus + Solution[1]; //don't allow the anchor points to be inside a sprocket
Y1Plus = (Y1Plus < R) ? R : Y1Plus;
Y2Plus = Y2Plus + Solution[2]; //don't allow the anchor points to be inside a sprocke
Y2Plus = (Y2Plus < R) ? R : Y2Plus;
Psi1 = Theta - Phi;
Psi2 = Theta + Phi;
Tries++; // increment itteration count
}
//Variables are within accuracy limits
// perform output computation
Offsetx1 = h * CosPsi1;
Offsetx2 = h * CosPsi2;
Offsety1 = h * SinPsi1;
Offsety2 = h * SinPsi2;
TanGamma = (y - Offsety1 + Y1Plus)/(x - Offsetx1);
TanLambda = (y - Offsety2 + Y2Plus)/(sysSettings.distBetweenMotors -(x + Offsetx2));
Gamma = atan(TanGamma);
Lambda =atan(TanLambda);
//compute the chain lengths
if(Mirror){
Chain2 = sqrt((x - Offsetx1)*(x - Offsetx1) + (y + Y1Plus - Offsety1)*(y + Y1Plus - Offsety1)) - R * TanGamma + R * Gamma; //right chain length
Chain1 = sqrt((sysSettings.distBetweenMotors - (x + Offsetx2))*(sysSettings.distBetweenMotors - (x + Offsetx2))+(y + Y2Plus - Offsety2)*(y + Y2Plus - Offsety2)) - R * TanLambda + R * Lambda; //left chain length
}
else{
Chain1 = sqrt((x - Offsetx1)*(x - Offsetx1) + (y + Y1Plus - Offsety1)*(y + Y1Plus - Offsety1)) - R * TanGamma + R * Gamma; //left chain length
Chain2 = sqrt((sysSettings.distBetweenMotors - (x + Offsetx2))*(sysSettings.distBetweenMotors - (x + Offsetx2))+(y + Y2Plus - Offsety2)*(y + Y2Plus - Offsety2)) - R * TanLambda + R * Lambda; //right chain length
}
*aChainLength = Chain1;
*bChainLength = Chain2;
}