cpConstraint *cpSpaceSerializer::createPivotJoint(TiXmlElement *elm)
{
cpConstraint *constraint;
cpBody *a;
cpBody *b;
createBodies(elm, &a, &b);
if (elm->FirstChildElement("worldAnchor"))
{
cpVect worldPt = createPoint("worldAnchor", elm);
constraint = cpPivotJointNew(a, b, worldPt);
}
else
{
cpVect anchr1 = createPoint("anchr1", elm);
cpVect anchr2 = createPoint("anchr2", elm);
constraint = cpPivotJointNew2(a, b, anchr1, anchr2);
}
//((cpPivotJoint*)constraint)->jAcc = createPoint("jAcc", elm);
return constraint;
}
bool PhysicsJointFixed::init(PhysicsBody* a, PhysicsBody* b, const Vec2& anchr)
{
do
{
CC_BREAK_IF(!PhysicsJoint::init(a, b));
getBodyNode(a)->setPosition(anchr);
getBodyNode(b)->setPosition(anchr);
// add a pivot joint to fixed two body together
auto constraint = cpPivotJointNew(a->getCPBody(), b->getCPBody(),
PhysicsHelper::point2cpv(anchr));
CC_BREAK_IF(constraint == nullptr);
_cpConstraints.push_back(constraint);
// add a gear joint to make two body have the same rotation.
constraint = cpGearJointNew(a->getCPBody(), b->getCPBody(), 0, 1);
CC_BREAK_IF(constraint == nullptr);
_cpConstraints.push_back(constraint);
setCollisionEnable(false);
return true;
} while (false);
return false;
}
bool PhysicsJointFixed::init(PhysicsBody* a, PhysicsBody* b, const Point& anchr)
{
do
{
CC_BREAK_IF(!PhysicsJoint::init(a, b));
getBodyNode(a)->setPosition(anchr);
getBodyNode(b)->setPosition(anchr);
// add a pivot joint to fixed two body together
cpConstraint* joint = cpPivotJointNew(getBodyInfo(a)->getBody(), getBodyInfo(b)->getBody(),
PhysicsHelper::point2cpv(anchr));
CC_BREAK_IF(joint == nullptr);
m_pInfo->add(joint);
// add a gear joint to make two body have the same rotation.
joint = cpGearJointNew(getBodyInfo(a)->getBody(), getBodyInfo(b)->getBody(), 0, 1);
CC_BREAK_IF(joint == nullptr);
m_pInfo->add(joint);
setCollisionEnable(false);
return true;
} while (false);
return false;
}
bool PhysicsJointFixed::createConstraints()
{
do
{
_bodyA->getNode()->setPosition(_anchr);
_bodyB->getNode()->setPosition(_anchr);
// add a pivot joint to fixed two body together
auto joint = cpPivotJointNew(_bodyA->getCPBody(), _bodyB->getCPBody(),
PhysicsHelper::point2cpv(_anchr));
CC_BREAK_IF(joint == nullptr);
_cpConstraints.push_back(joint);
// add a gear joint to make two body have the same rotation.
joint = cpGearJointNew(_bodyA->getCPBody(), _bodyB->getCPBody(), 0, 1);
CC_BREAK_IF(joint == nullptr);
_cpConstraints.push_back(joint);
_collisionEnable = false;
return true;
} while (false);
return false;
}
static cpBool
StickyPreSolve(cpArbiter *arb, cpSpace *space, void *data)
{
// We want to fudge the collisions a bit to allow shapes to overlap more.
// This simulates their squishy sticky surface, and more importantly
// keeps them from separating and destroying the joint.
// Track the deepest collision point and use that to determine if a rigid collision should occur.
cpFloat deepest = INFINITY;
// Grab the contact set and iterate over them.
cpContactPointSet contacts = cpArbiterGetContactPointSet(arb);
for(int i=0; i<contacts.count; i++){
// Increase the distance (negative means overlaping) of the
// collision to allow them to overlap more.
// This value is used only for fixing the positions of overlapping shapes.
cpFloat dist = contacts.points[i].dist + 2.0f*STICK_SENSOR_THICKNESS;
contacts.points[i].dist = cpfmin(0.0f, dist);
deepest = cpfmin(deepest, dist);
}
// Set the new contact point data.
cpArbiterSetContactPointSet(arb, &contacts);
// If the shapes are overlapping enough, then create a
// joint that sticks them together at the first contact point.
if(!cpArbiterGetUserData(arb) && deepest <= 0.0f){
CP_ARBITER_GET_BODIES(arb, bodyA, bodyB);
// Create a joint at the contact point to hold the body in place.
cpConstraint *joint = cpPivotJointNew(bodyA, bodyB, contacts.points[0].point);
// Give it a finite force for the stickyness.
cpConstraintSetMaxForce(joint, 3e3);
// Schedule a post-step() callback to add the joint.
cpSpaceAddPostStepCallback(space, PostStepAddJoint, joint, NULL);
// Store the joint on the arbiter so we can remove it later.
cpArbiterSetUserData(arb, joint);
}
// Position correction and velocity are handled separately so changing
// the overlap distance alone won't prevent the collision from occuring.
// Explicitly the collision for this frame if the shapes don't overlap using the new distance.
return (deepest <= 0.0f);
// Lots more that you could improve upon here as well:
// * Modify the joint over time to make it plastic.
// * Modify the joint in the post-step to make it conditionally plastic (like clay).
// * Track a joint for the deepest contact point instead of the first.
// * Track a joint for each contact point. (more complicated since you only get one data pointer).
}
WorldConstraint_t *worldConstr_createPivotJoint(WorldEntity_t *a, WorldEntity_t *b, vec2_t aPivot)
{
dynamo_assert(a->world == b->world, "Entities are not in the same world");
WorldConstraint_t *ret = obj_create_autoreleased(&Class_WorldConstraint);
ret->world = a->world;
ret->a = obj_retain(a);
ret->b = obj_retain(b);
ret->type = kWorldJointType_Pivot;
ret->cpConstraint = cpPivotJointNew(a->cpBody, b->cpBody, VEC2_TO_CPV(aPivot));
cpSpaceAddConstraint(ret->world->cpSpace, ret->cpConstraint);
return ret;
}
bool PhysicsJointPin::init(PhysicsBody *a, PhysicsBody *b, const Vec2& anchr)
{
do
{
CC_BREAK_IF(!PhysicsJoint::init(a, b));
auto constraint = cpPivotJointNew(a->getCPBody(), b->getCPBody(),
PhysicsHelper::point2cpv(anchr));
CC_BREAK_IF(constraint == nullptr);
_cpConstraints.push_back(constraint);
return true;
} while (false);
return false;
}
bool PhysicsJointPin::init(PhysicsBody *a, PhysicsBody *b, const Point& anchr)
{
do
{
CC_BREAK_IF(!PhysicsJoint::init(a, b));
cpConstraint* joint = cpPivotJointNew(getBodyInfo(a)->getBody(), getBodyInfo(b)->getBody(),
PhysicsHelper::point2cpv(anchr));
CC_BREAK_IF(joint == nullptr);
m_pInfo->add(joint);
return true;
} while (false);
return false;
}
static cannon_t *createCannon(cpVect p, cpFloat length, cpFloat radius) {
cannon_t *c;
cpVect a, b;
a = cpv(0, 0);
b = cpv(length, 0);
c = (cannon_t *)malloc(sizeof(cannon_t));
c->length = length;
c->body = cpSpaceAddBody(g_Space, cpBodyNew(2.0f, INFINITY));
c->body->p = p;
/*c->shape = cpPolyShapeNew(c->body, NUM_DOMINO_VERTS, dominoVerts, cpvzero);*/
c->shape = cpSpaceAddShape(g_Space, cpSegmentShapeNew(c->body, a, b, radius));
c->shape->e = 0.0f;
c->shape->u = 1.0f;
cpSpaceAddConstraint(g_Space, cpPivotJointNew(c->body, &g_Space->staticBody, p));
return c;
}
int MagneticGripperGrippableCollisionPreSolve(cpArbiter* pt_arb, cpSpace* pt_space, void* p_data) {
/* Get the shapes involved */
CP_ARBITER_GET_SHAPES(pt_arb, ptGripperShape, ptGrippableShape);
/* Get a reference to the gripper data */
SDynamics2DEngineGripperData& sGripperData = *reinterpret_cast<SDynamics2DEngineGripperData*>(ptGripperShape->data);
/* The gripper is locked or unlocked? */
if(sGripperData.GripperEntity.IsUnlocked()) {
/* The gripper is locked. If it was gripping an object,
* release it. Then, process the collision normally */
if(sGripperData.GripperEntity.IsGripping()) {
sGripperData.ClearConstraints();
}
return 1;
}
else if(! sGripperData.GripperEntity.IsGripping()) {
/* The gripper is unlocked and free, create the joints */
/* Prevent gripper from slipping */
pt_arb->e = 0.0f; // No elasticity
pt_arb->u = 1.0f; // Max friction
pt_arb->surface_vr = cpvzero; // No surface velocity
/* Calculate the anchor point on the grippable body
as the centroid of the contact points */
cpVect tGrippableAnchor = cpvzero;
for(SInt32 i = 0; i < pt_arb->numContacts; ++i) {
tGrippableAnchor = cpvadd(tGrippableAnchor, cpArbiterGetPoint(pt_arb, i));
}
tGrippableAnchor = cpvmult(tGrippableAnchor, 1.0f / pt_arb->numContacts);
/* Create a constraint */
sGripperData.GripConstraint = cpSpaceAddConstraint(pt_space,
cpPivotJointNew(
ptGripperShape->body,
ptGrippableShape->body,
tGrippableAnchor));
sGripperData.GripConstraint->biasCoef = 0.95f; // Correct overlap
sGripperData.GripConstraint->maxBias = 0.01f; // Max correction speed
sGripperData.GripperEntity.SetGrippedEntity(*reinterpret_cast<CEmbodiedEntity*>(ptGrippableShape->data));
}
/* Ignore the collision, the objects are gripped already */
return 0;
}
bool PhysicsJointPin::createConstraints()
{
do
{
cpConstraint* joint = nullptr;
if (_useSpecificAnchr)
{
joint = cpPivotJointNew2(_bodyA->getCPBody(), _bodyB->getCPBody(),
PhysicsHelper::point2cpv(_anchr1), PhysicsHelper::point2cpv(_anchr2));
}
else
{
joint = cpPivotJointNew(_bodyA->getCPBody(), _bodyB->getCPBody(),
PhysicsHelper::point2cpv(_anchr1));
}
CC_BREAK_IF(joint == nullptr);
_cpConstraints.push_back(joint);
return true;
} while (false);
return false;
}
static cpSpace *
init(void)
{
ChipmunkDemoMessageString = "Control the crane by moving the mouse. Press the down arrow to release.";
space = cpSpaceNew();
cpSpaceSetIterations(space, 30);
cpSpaceSetGravity(space, cpv(0, -100));
cpSpaceSetDamping(space, 0.8);
cpBody *staticBody = cpSpaceGetStaticBody(space);
cpShape *shape;
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(320,-240), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetLayers(shape, NOT_GRABABLE_MASK);
// Add a body for the dolly.
dollyBody = cpSpaceAddBody(space, cpBodyNew(10, INFINITY));
cpBodySetPos(dollyBody, cpv(0, 100));
// Add a block so you can see it.
cpSpaceAddShape(space, cpBoxShapeNew(dollyBody, 30, 30));
// Add a groove joint for it to move back and forth on.
cpSpaceAddConstraint(space, cpGrooveJointNew(staticBody, dollyBody, cpv(-250, 100), cpv(250, 100), cpvzero));
// Add a pivot joint to act as a servo motor controlling it's position
// By updating the anchor points of the pivot joint, you can move the dolly.
dollyServo = cpSpaceAddConstraint(space, cpPivotJointNew(staticBody, dollyBody, cpBodyGetPos(dollyBody)));
// Max force the dolly servo can generate.
cpConstraintSetMaxForce(dollyServo, 10000);
// Max speed of the dolly servo
cpConstraintSetMaxBias(dollyServo, 100);
// You can also change the error bias to control how it slows down.
//cpConstraintSetErrorBias(dollyServo, 0.2);
// Add the crane hook.
cpBody *hookBody = cpSpaceAddBody(space, cpBodyNew(1, INFINITY));
cpBodySetPos(hookBody, cpv(0, 50));
// Add a sensor shape for it. This will be used to figure out when the hook touches a box.
shape = cpSpaceAddShape(space, cpCircleShapeNew(hookBody, 10, cpvzero));
cpShapeSetSensor(shape, cpTrue);
cpShapeSetCollisionType(shape, HOOK_SENSOR);
// Add a slide joint to act as a winch motor
// By updating the max length of the joint you can make it pull up the load.
winchServo = cpSpaceAddConstraint(space, cpSlideJointNew(dollyBody, hookBody, cpvzero, cpvzero, 0, INFINITY));
// Max force the dolly servo can generate.
cpConstraintSetMaxForce(winchServo, 30000);
// Max speed of the dolly servo
cpConstraintSetMaxBias(winchServo, 60);
// TODO cleanup
// Finally a box to play with
cpBody *boxBody = cpSpaceAddBody(space, cpBodyNew(30, cpMomentForBox(30, 50, 50)));
cpBodySetPos(boxBody, cpv(200, -200));
// Add a block so you can see it.
shape = cpSpaceAddShape(space, cpBoxShapeNew(boxBody, 50, 50));
cpShapeSetFriction(shape, 0.7);
cpShapeSetCollisionType(shape, CRATE);
cpSpaceAddCollisionHandler(space, HOOK_SENSOR, CRATE, (cpCollisionBeginFunc)HookCrate, NULL, NULL, NULL, NULL);
return space;
}
static void
AttachHook(cpSpace *space, cpBody *hook, cpBody *crate)
{
hookJoint = cpSpaceAddConstraint(space, cpPivotJointNew(hook, crate, cpBodyGetPos(hook)));
}
cpJoint * bmx_cppivotjoint_create(BBObject * handle, cpBody * bodyA, cpBody * bodyB, cpVect * pivot) {
cpJoint * joint = cpPivotJointNew(bodyA, bodyB, *pivot);
cpbind(joint, handle);
return joint;
}
PivotJoint::PivotJoint(std::shared_ptr<Body> bodyA,
std::shared_ptr<Body> bodyB,
cpVect pivot) :
Constraint(cpPivotJointNew(*bodyA, *bodyB, pivot),
bodyA, bodyB)
{}
static cpSpace *
init(void)
{
space = cpSpaceNew();
cpSpaceSetIterations(space, 10);
cpSpaceSetGravity(space, cpv(0, -100));
cpSpaceSetSleepTimeThreshold(space, 0.5f);
cpBody *staticBody = cpSpaceGetStaticBody(space);
cpShape *shape;
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-320,240), cpv(320,240), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetLayers(shape, NOT_GRABABLE_MASK);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-320,120), cpv(320,120), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetLayers(shape, NOT_GRABABLE_MASK);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-320,0), cpv(320,0), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetLayers(shape, NOT_GRABABLE_MASK);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-320,-120), cpv(320,-120), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetLayers(shape, NOT_GRABABLE_MASK);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(320,-240), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetLayers(shape, NOT_GRABABLE_MASK);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(-320,240), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetLayers(shape, NOT_GRABABLE_MASK);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-160,-240), cpv(-160,240), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetLayers(shape, NOT_GRABABLE_MASK);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(0,-240), cpv(0,240), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetLayers(shape, NOT_GRABABLE_MASK);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(160,-240), cpv(160,240), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetLayers(shape, NOT_GRABABLE_MASK);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(320,-240), cpv(320,240), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetLayers(shape, NOT_GRABABLE_MASK);
cpVect boxOffset;
cpBody *body1, *body2;
cpVect posA = cpv( 50, 60);
cpVect posB = cpv(110, 60);
#define POS_A cpvadd(boxOffset, posA)
#define POS_B cpvadd(boxOffset, posB)
// Pin Joints - Link shapes with a solid bar or pin.
// Keeps the anchor points the same distance apart from when the joint was created.
boxOffset = cpv(-320, -240);
body1 = addBall(posA, boxOffset);
body2 = addBall(posB, boxOffset);
cpSpaceAddConstraint(space, cpPinJointNew(body1, body2, cpv(15,0), cpv(-15,0)));
// Slide Joints - Like pin joints but with a min/max distance.
// Can be used for a cheap approximation of a rope.
boxOffset = cpv(-160, -240);
body1 = addBall(posA, boxOffset);
body2 = addBall(posB, boxOffset);
cpSpaceAddConstraint(space, cpSlideJointNew(body1, body2, cpv(15,0), cpv(-15,0), 20.0f, 40.0f));
// Pivot Joints - Holds the two anchor points together. Like a swivel.
boxOffset = cpv(0, -240);
body1 = addBall(posA, boxOffset);
body2 = addBall(posB, boxOffset);
cpSpaceAddConstraint(space, cpPivotJointNew(body1, body2, cpvadd(boxOffset, cpv(80,60))));
// cpPivotJointNew() takes it's anchor parameter in world coordinates. The anchors are calculated from that
// cpPivotJointNew2() lets you specify the two anchor points explicitly
// Groove Joints - Like a pivot joint, but one of the anchors is a line segment that the pivot can slide in
boxOffset = cpv(160, -240);
body1 = addBall(posA, boxOffset);
body2 = addBall(posB, boxOffset);
cpSpaceAddConstraint(space, cpGrooveJointNew(body1, body2, cpv(30,30), cpv(30,-30), cpv(-30,0)));
// Damped Springs
boxOffset = cpv(-320, -120);
body1 = addBall(posA, boxOffset);
body2 = addBall(posB, boxOffset);
cpSpaceAddConstraint(space, cpDampedSpringNew(body1, body2, cpv(15,0), cpv(-15,0), 20.0f, 5.0f, 0.3f));
// Damped Rotary Springs
boxOffset = cpv(-160, -120);
body1 = addBar(posA, boxOffset);
body2 = addBar(posB, boxOffset);
// Add some pin joints to hold the circles in place.
cpSpaceAddConstraint(space, cpPivotJointNew(body1, staticBody, POS_A));
cpSpaceAddConstraint(space, cpPivotJointNew(body2, staticBody, POS_B));
cpSpaceAddConstraint(space, cpDampedRotarySpringNew(body1, body2, 0.0f, 3000.0f, 60.0f));
// Rotary Limit Joint
boxOffset = cpv(0, -120);
body1 = addLever(posA, boxOffset);
body2 = addLever(posB, boxOffset);
// Add some pin joints to hold the circles in place.
cpSpaceAddConstraint(space, cpPivotJointNew(body1, staticBody, POS_A));
cpSpaceAddConstraint(space, cpPivotJointNew(body2, staticBody, POS_B));
// Hold their rotation within 90 degrees of each other.
cpSpaceAddConstraint(space, cpRotaryLimitJointNew(body1, body2, -M_PI_2, M_PI_2));
// Ratchet Joint - A rotary ratchet, like a socket wrench
boxOffset = cpv(160, -120);
body1 = addLever(posA, boxOffset);
body2 = addLever(posB, boxOffset);
// Add some pin joints to hold the circles in place.
cpSpaceAddConstraint(space, cpPivotJointNew(body1, staticBody, POS_A));
cpSpaceAddConstraint(space, cpPivotJointNew(body2, staticBody, POS_B));
// Ratchet every 90 degrees
cpSpaceAddConstraint(space, cpRatchetJointNew(body1, body2, 0.0f, M_PI_2));
// Gear Joint - Maintain a specific angular velocity ratio
boxOffset = cpv(-320, 0);
body1 = addBar(posA, boxOffset);
body2 = addBar(posB, boxOffset);
// Add some pin joints to hold the circles in place.
cpSpaceAddConstraint(space, cpPivotJointNew(body1, staticBody, POS_A));
cpSpaceAddConstraint(space, cpPivotJointNew(body2, staticBody, POS_B));
// Force one to sping 2x as fast as the other
cpSpaceAddConstraint(space, cpGearJointNew(body1, body2, 0.0f, 2.0f));
// Simple Motor - Maintain a specific angular relative velocity
boxOffset = cpv(-160, 0);
body1 = addBar(posA, boxOffset);
body2 = addBar(posB, boxOffset);
// Add some pin joints to hold the circles in place.
cpSpaceAddConstraint(space, cpPivotJointNew(body1, staticBody, POS_A));
cpSpaceAddConstraint(space, cpPivotJointNew(body2, staticBody, POS_B));
// Make them spin at 1/2 revolution per second in relation to each other.
cpSpaceAddConstraint(space, cpSimpleMotorNew(body1, body2, M_PI));
// Make a car with some nice soft suspension
boxOffset = cpv(0, 0);
cpBody *wheel1 = addWheel(posA, boxOffset);
cpBody *wheel2 = addWheel(posB, boxOffset);
cpBody *chassis = addChassis(cpv(80, 100), boxOffset);
cpSpaceAddConstraint(space, cpGrooveJointNew(chassis, wheel1, cpv(-30, -10), cpv(-30, -40), cpvzero));
cpSpaceAddConstraint(space, cpGrooveJointNew(chassis, wheel2, cpv( 30, -10), cpv( 30, -40), cpvzero));
cpSpaceAddConstraint(space, cpDampedSpringNew(chassis, wheel1, cpv(-30, 0), cpvzero, 50.0f, 20.0f, 10.0f));
cpSpaceAddConstraint(space, cpDampedSpringNew(chassis, wheel2, cpv( 30, 0), cpvzero, 50.0f, 20.0f, 10.0f));
return space;
}
