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); }