Verlet Integration 2D Physic, implementing angular constraint

877 Views Asked by At

I am trying to implement an angular constraint into a simple verlet integration based 2D physic engine. This is the code I am currently using:

int indexA = physicAngularConstraint[i].indexA;
int indexB = physicAngularConstraint[i].indexB;
int indexC = physicAngularConstraint[i].indexC;

CGPoint e = CGPointSubtract(physicParticle[indexB].pos,physicParticle[indexA].pos);
CGPoint f = CGPointSubtract(physicParticle[indexC].pos,physicParticle[indexB].pos);

float dot = CGPointDot(e, f);
float cross = CGPointCross(e, f);
float angle = atan2f(cross, dot);

float da = (angle < physicAngularConstraint[i].minAngle)? angle - physicAngularConstraint[i].minAngle : (angle > physicAngularConstraint[i].maxAngle)? angle - physicAngularConstraint[i].maxAngle : 0.0f;

if (da != 0.0f)
{
    physicParticle[indexA].pos = CGPointRotate(physicParticle[indexA].pos,
                                               physicParticle[indexB].pos, da);
    physicParticle[indexC].pos = CGPointRotate(physicParticle[indexC].pos,
                                               physicParticle[indexB].pos, -da);
}

The CGPointRotate function looks like this:

CGPoint CGPointRotate(CGPoint pt, CGPoint center, float angle)
{
    CGPoint ret;

    pt = CGPointSubtract(pt, center);

    float co = cosf(angle);
    float si = sinf(angle);
    ret.x = pt.x * co - pt.y * si;
    ret.y = pt.x * si + pt.y * co;

    ret = CGPointAdd(ret, center);

    return ret;
}

I am testing this implementation with a row of particles which are connected through distant constraints. Without the angular constraints they act like a rope. I am trying to give the "rope" some stiffness through the angular constraints. But my implementation above is gaining energy and blowing up the system after some millisecs. Why does this constrain implementation is gaining energie?

0

There are 0 best solutions below