static void DrawConstraint(cpConstraint *constraint, CCDrawNode *renderer)
{
cpBody *body_a = constraint->a;
cpBody *body_b = constraint->b;
const cpConstraintClass *klass = constraint->CP_PRIVATE(klass);
if (klass == cpPinJointGetClass())
{
cpPinJoint *joint = (cpPinJoint *)constraint;
cpVect a = cpBodyLocal2World(body_a, joint->anchr1);
cpVect b = cpBodyLocal2World(body_b, joint->anchr2);
renderer->drawDot(cpVert2ccp(a), 3.0, CONSTRAINT_COLOR);
renderer->drawDot(cpVert2ccp(b), 3.0, CONSTRAINT_COLOR);
renderer->drawSegment(cpVert2ccp(a), cpVert2ccp(b), 1.0, CONSTRAINT_COLOR);
}
else if (klass == cpSlideJointGetClass())
{
cpSlideJoint *joint = (cpSlideJoint *)constraint;
cpVect a = cpBodyLocal2World(body_a, joint->anchr1);
cpVect b = cpBodyLocal2World(body_b, joint->anchr2);
renderer->drawDot(cpVert2ccp(a), 3.0, CONSTRAINT_COLOR);
renderer->drawDot(cpVert2ccp(b), 3.0, CONSTRAINT_COLOR);
renderer->drawSegment(cpVert2ccp(a), cpVert2ccp(b), 1.0, CONSTRAINT_COLOR);
}
else if (klass == cpPivotJointGetClass())
{
cpPivotJoint *joint = (cpPivotJoint *)constraint;
cpVect a = cpBodyLocal2World(body_a, joint->anchr1);
cpVect b = cpBodyLocal2World(body_b, joint->anchr2);
renderer->drawDot(cpVert2ccp(a), 3.0, CONSTRAINT_COLOR);
renderer->drawDot(cpVert2ccp(b), 3.0, CONSTRAINT_COLOR);
}
else if (klass == cpGrooveJointGetClass())
{
cpGrooveJoint *joint = (cpGrooveJoint *)constraint;
cpVect a = cpBodyLocal2World(body_a, joint->grv_a);
cpVect b = cpBodyLocal2World(body_a, joint->grv_b);
cpVect c = cpBodyLocal2World(body_b, joint->anchr2);
renderer->drawDot(cpVert2ccp(c), 3.0, CONSTRAINT_COLOR);
renderer->drawSegment(cpVert2ccp(a), cpVert2ccp(b), 1.0, CONSTRAINT_COLOR);
}
else if (klass == cpDampedSpringGetClass())
{
// TODO
}
else
{
// printf("Cannot draw constraint\n");
}
}
cpBody* drawing_update(cpSpace *space, cpBody *drawing, cpFloat x, cpFloat y) {
Body_data *pa = cpBodyGetUserData(drawing);
int length = pa->x_values->len;
cpVect old = cpvzero;
old.x = g_array_index(pa->x_values, cpFloat, length - 1);
old.y = g_array_index(pa->y_values, cpFloat, length - 1);
old = cpBodyLocal2World(drawing, old);
cpVect point = cpv(x, y);
// if the difference between the new x and the previous x is greater than the threshold
if (cpvdistsq(old, point) > THRESHOLD) {
cpVect position = cpBodyGetPos(drawing);
point = cpvsub(point, position);
//cpBodyWorld2Local(drawing, point);
fprintf(stdout, "Point= (%5.2f, %5.2f) while x= %5.2f, y=%5.2f\n", point.x, point.y, x, y);
x = point.x;
y = point.y;
g_array_append_val(pa->x_values, x);
g_array_append_val(pa->y_values, y);
cpBodySetUserData(drawing, pa);
drawing = add_segment_to_body(space, drawing);
}
cpBodySetVel(drawing, cpvzero);
return drawing;
}
static void
draw_poly_shape(struct draw_options opts, cpShape *shape, cpBody *body)
{
int num_verts = cpPolyShapeGetNumVerts(shape);
cpVect first_vert = cpBodyLocal2World(body, cpPolyShapeGetVert(shape, 0));
cpVect prev_vert = first_vert;
cpVect pos = cpBodyGetPos(body);
for (int i = 1; i < num_verts; i++)
{
cpVect vert = cpBodyLocal2World(body, cpPolyShapeGetVert(shape, i));
draw_line(opts, prev_vert, vert);
prev_vert = vert;
}
/* close up the polygon */
draw_line(opts, prev_vert, first_vert);
}
static void
preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
// calculate endpoints in worldspace
cpVect ta = cpBodyLocal2World(a, joint->grv_a);
cpVect tb = cpBodyLocal2World(a, joint->grv_b);
// calculate axis
cpVect n = cpvrotate(joint->grv_n, a->rot);
cpFloat d = cpvdot(ta, n);
joint->grv_tn = n;
joint->r2 = cpvrotate(joint->anchr2, b->rot);
// calculate tangential distance along the axis of r2
cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n);
// calculate clamping factor and r2
if(td <= cpvcross(ta, n)){
joint->clamp = 1.0f;
joint->r1 = cpvsub(ta, a->p);
} else if(td >= cpvcross(tb, n)){
joint->clamp = -1.0f;
joint->r1 = cpvsub(tb, a->p);
} else {
joint->clamp = 0.0f;
joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
}
// Calculate mass tensor
k_tensor(a, b, joint->r1, joint->r2, &joint->k1, &joint->k2);
// compute max impulse
joint->jMaxLen = J_MAX(joint, dt);
// calculate bias velocity
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias);
// apply accumulated impulse
apply_impulses(a, b, joint->r1, joint->r2, joint->jAcc);
}
static void
ClipPoly(cpSpace *space, cpShape *shape, cpVect n, cpFloat dist)
{
cpBody *body = cpShapeGetBody(shape);
int count = cpPolyShapeGetNumVerts(shape);
int clippedCount = 0;
cpVect *clipped = (cpVect *)alloca((count + 1)*sizeof(cpVect));
for(int i=0, j=count-1; i<count; j=i, i++){
cpVect a = cpBodyLocal2World(body, cpPolyShapeGetVert(shape, j));
cpFloat a_dist = cpvdot(a, n) - dist;
if(a_dist < 0.0){
clipped[clippedCount] = a;
clippedCount++;
}
cpVect b = cpBodyLocal2World(body, cpPolyShapeGetVert(shape, i));
cpFloat b_dist = cpvdot(b, n) - dist;
if(a_dist*b_dist < 0.0f){
cpFloat t = cpfabs(a_dist)/(cpfabs(a_dist) + cpfabs(b_dist));
clipped[clippedCount] = cpvlerp(a, b, t);
clippedCount++;
}
}
cpVect centroid = cpCentroidForPoly(clippedCount, clipped);
cpFloat mass = cpAreaForPoly(clippedCount, clipped)*DENSITY;
cpFloat moment = cpMomentForPoly(mass, clippedCount, clipped, cpvneg(centroid));
cpBody *new_body = cpSpaceAddBody(space, cpBodyNew(mass, moment));
cpBodySetPos(new_body, centroid);
cpBodySetVel(new_body, cpBodyGetVelAtWorldPoint(body, centroid));
cpBodySetAngVel(new_body, cpBodyGetAngVel(body));
cpShape *new_shape = cpSpaceAddShape(space, cpPolyShapeNew(new_body, clippedCount, clipped, cpvneg(centroid)));
// Copy whatever properties you have set on the original shape that are important
cpShapeSetFriction(new_shape, cpShapeGetFriction(shape));
}
static cpBody *utils_update_drawing(cpBody *drawing) {
cpFloat mass = cpBodyGetMass(drawing);
cpFloat moment = cpBodyGetMoment(drawing);
Body_data *pa = cpBodyGetUserData(drawing);
//cpFloat x = g_array_index(pa->x_values, cpFloat, 0);
//cpFloat y = g_array_index(pa->y_values, cpFloat, 0);
cpVect pos_a, pos_b;
cpVect origin = cpBodyGetPos(drawing);
cpFloat mi, micx = 0, micy = 0;
int length = pa->x_values->len;
for (int index = 1; index < length; index++) {
pos_a = cpv(g_array_index(pa->x_values, cpFloat, index - 1), g_array_index(pa->y_values, cpFloat, index - 1));
pos_b = cpv(g_array_index(pa->x_values, cpFloat, index), g_array_index(pa->y_values, cpFloat, index));
//fprintf(stdout, "Pos_a = (%5.2f, %5.2f)\n", pos_a.x, pos_a.y);
mi = (CRAYON_MASS * cpAreaForSegment( pos_a, pos_b, CRAYON_RADIUS ));
micx += mi * ((pos_a.x + pos_b.x) / 2);
micy += mi * ((pos_a.y + pos_b.y) / 2);
mass += mi;
moment += cpMomentForSegment(mass, pos_a, pos_b); // not actually sum, but maybe it is
}
cpBodySetMass(drawing, mass);
cpBodySetMoment(drawing, moment);
// center of mass is the average of all vertices NOT
//cpVect new_origin = cpv(x / length, y / length);
cpVect new_origin = cpv(micx / mass, micy / mass);
new_origin = cpBodyLocal2World(drawing, new_origin);
cpBodySetPos( drawing, new_origin );
//fprintf(stdout, "Position set at (%5.2f, %5.2f)\n", new_origin.x, new_origin.y);
cpSpace * space = cpBodyGetSpace(drawing);
cpSpaceReindexShapesForBody(space, drawing);
//cpBodySetPos(drawing, cpv(pos.x + (second.x / length), pos.y + (second.y / length)));
//pa->offset = cpvsub(new_origin, origin);
pa = shift_origin(drawing, origin, new_origin);
cpBodySetUserData(drawing, pa);
if (space)
post_step_body_replace_shapes(space, drawing, NULL);
else
fprintf(stderr, "WTF\n");
//if (!(cpSpaceAddPostStepCallback(space, (cpPostStepFunc) post_step_body_replace_shapes, drawing, NULL)))
//fprintf(stderr, "FAILED POST-STEP CALLBACK\n\n");
return drawing;
}
static void
preStep(cpGrooveJoint *joint, cpFloat dt)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
// calculate endpoints in worldspace
cpVect ta = cpBodyLocal2World(a, joint->grv_a);
cpVect tb = cpBodyLocal2World(a, joint->grv_b);
// calculate axis
cpVect n = cpvrotate(joint->grv_n, a->rot);
cpFloat d = cpvdot(ta, n);
joint->grv_tn = n;
joint->r2 = cpvrotate(joint->anchr2, b->rot);
// calculate tangential distance along the axis of r2
cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n);
// calculate clamping factor and r2
if(td <= cpvcross(ta, n)){
joint->clamp = 1.0f;
joint->r1 = cpvsub(ta, a->p);
} else if(td >= cpvcross(tb, n)){
joint->clamp = -1.0f;
joint->r1 = cpvsub(tb, a->p);
} else {
joint->clamp = 0.0f;
joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
}
// Calculate mass tensor
joint->k = k_tensor(a, b, joint->r1, joint->r2);
// calculate bias velocity
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias);
}
TiXmlElement *cpSpaceSerializer::createPivotJointElm(cpPivotJoint *constraint)
{
TiXmlElement *elm = new TiXmlElement("constraint");
setAttribute(elm, "type", "pivot");
elm->LinkEndChild(createPointElm("anchr1", constraint->anchr1));
elm->LinkEndChild(createPointElm("anchr2", constraint->anchr2));
cpVect wPt = cpBodyLocal2World(((cpConstraint*)(constraint))->a, constraint->anchr1);
elm->LinkEndChild(createPointElm("worldAnchor", wPt));
elm->LinkEndChild(createPointElm("jAcc", constraint->jAcc));
return elm;
}
static void
update(cpSpace *space)
{
cpFloat tolerance = 2.0;
if(ChipmunkDemoRightClick && cpShapeNearestPointQuery(shape, ChipmunkDemoMouse, NULL) > tolerance){
cpBody *body = cpShapeGetBody(shape);
int count = cpPolyShapeGetNumVerts(shape);
// Allocate the space for the new vertexes on the stack.
cpVect *verts = (cpVect *)alloca((count + 1)*sizeof(cpVect));
for(int i=0; i<count; i++){
verts[i] = cpPolyShapeGetVert(shape, i);
}
verts[count] = cpBodyWorld2Local(body, ChipmunkDemoMouse);
// This function builds a convex hull for the vertexes.
// Because the result array is NULL, it will reduce the input array instead.
int hullCount = cpConvexHull(count + 1, verts, NULL, NULL, tolerance);
// Figure out how much to shift the body by.
cpVect centroid = cpCentroidForPoly(hullCount, verts);
// Recalculate the body properties to match the updated shape.
cpFloat mass = cpAreaForPoly(hullCount, verts)*DENSITY;
cpBodySetMass(body, mass);
cpBodySetMoment(body, cpMomentForPoly(mass, hullCount, verts, cpvneg(centroid)));
cpBodySetPos(body, cpBodyLocal2World(body, centroid));
// Use the setter function from chipmunk_unsafe.h.
// You could also remove and recreate the shape if you wanted.
cpPolyShapeSetVerts(shape, hullCount, verts, cpvneg(centroid));
}
int steps = 1;
cpFloat dt = 1.0f/60.0f/(cpFloat)steps;
for(int i=0; i<steps; i++){
cpSpaceStep(space, dt);
}
}
cVect cBody::Local2World( const cVect v ) {
return tovect( cpBodyLocal2World( mBody, tocpv( v ) ) );
}
Vec2 PhysicsBody::local2World(const Vec2& point)
{
return PhysicsHelper::cpv2point(cpBodyLocal2World(_info->getBody(), PhysicsHelper::point2cpv(point)));
}
cpVect * bmx_cpbody_local2world(cpBody * body, cpVect * vec) {
return bmx_cpvect_new(cpBodyLocal2World(body, *vec));
}
void wrBodyLocal2World(cpBody *b, cpVect *v) {
cpVect ret = cpBodyLocal2World(b, *v);
v->x = ret.x;
v->y = ret.y;
}
static VALUE
rb_cpBodyLocal2World(VALUE self, VALUE v) {
return VNEW(cpBodyLocal2World(BODY(self), *VGET(v)));
}
static int cpBody_getLocal2World (lua_State *L){
cpBody *b = check_cpBody(L, 1);
cpVect vi = check_cpVect(L, 2);
push_cpVect(L, cpBodyLocal2World(b, vi));
return 2;
}
cp::Vect Body::local2World(const cp::Vect& v)
{
return cpBodyLocal2World(body,v);
}
