void CDynamics2DVelocityControl::AttachTo(cpBody* pt_body) {
/* If we are already controlling a body, detach from it first */
if(m_ptControlledBody != NULL) {
Detach();
}
/* Set the wanted body as the new controlled one */
m_ptControlledBody = pt_body;
/* Add linear constraint */
m_ptLinearConstraint =
cpSpaceAddConstraint(m_cDyn2DEngine.GetPhysicsSpace(),
cpPivotJointNew2(m_ptControlledBody,
m_ptControlBody,
cpvzero,
cpvzero));
m_ptLinearConstraint->maxBias = 0.0f; /* disable joint correction */
m_ptLinearConstraint->maxForce = m_fMaxForce; /* limit the dragging force */
/* Add angular constraint */
m_ptAngularConstraint =
cpSpaceAddConstraint(m_cDyn2DEngine.GetPhysicsSpace(),
cpGearJointNew(m_ptControlledBody,
m_ptControlBody,
0.0f,
1.0f));
m_ptAngularConstraint->maxBias = 0.0f; /* disable joint correction */
m_ptAngularConstraint->maxForce = m_fMaxTorque; /* limit the torque */
}
static cpSpace *
init(void)
{
staticBody = cpBodyNew(INFINITY, INFINITY);
cpResetShapeIdCounter();
space = cpSpaceNew();
cpSpaceResizeActiveHash(space, 30.0f, 1000);
space->iterations = 10;
cpShape *shape;
// Create segments around the edge of the screen.
shape = cpSpaceAddStaticShape(space, cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(-320,240), 0.0f));
shape->e = 1.0f; shape->u = 1.0f;
shape->layers = NOT_GRABABLE_MASK;
shape = cpSpaceAddStaticShape(space, cpSegmentShapeNew(staticBody, cpv(320,-240), cpv(320,240), 0.0f));
shape->e = 1.0f; shape->u = 1.0f;
shape->layers = NOT_GRABABLE_MASK;
shape = cpSpaceAddStaticShape(space, cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(320,-240), 0.0f));
shape->e = 1.0f; shape->u = 1.0f;
shape->layers = NOT_GRABABLE_MASK;
shape = cpSpaceAddStaticShape(space, cpSegmentShapeNew(staticBody, cpv(-320,240), cpv(320,240), 0.0f));
shape->e = 1.0f; shape->u = 1.0f;
shape->layers = NOT_GRABABLE_MASK;
for(int i=0; i<50; i++){
cpBody *body = add_box(10.0, 1.0);
cpConstraint *pivot = cpSpaceAddConstraint(space, cpPivotJointNew2(staticBody, body, cpvzero, cpvzero));
pivot->biasCoef = 0.0f; // disable joint correction
pivot->maxForce = 1000.0f; // emulate linear friction
cpConstraint *gear = cpSpaceAddConstraint(space, cpGearJointNew(staticBody, body, 0.0f, 1.0f));
gear->biasCoef = 0.0f; // disable joint correction
gear->maxForce = 5000.0f; // emulate angular friction
}
// We joint the tank to the control body and control the tank indirectly by modifying the control body.
tankControlBody = cpBodyNew(INFINITY, INFINITY);
tankBody = add_box(15.0, 10.0);
cpConstraint *pivot = cpSpaceAddConstraint(space, cpPivotJointNew2(tankControlBody, tankBody, cpvzero, cpvzero));
pivot->biasCoef = 0.0f; // disable joint correction
pivot->maxForce = 10000.0f; // emulate linear friction
cpConstraint *gear = cpSpaceAddConstraint(space, cpGearJointNew(tankControlBody, tankBody, 0.0f, 1.0f));
gear->biasCoef = 1.0f; // limit angular correction rate
gear->maxBias = 1.0f; // limit angular correction rate
gear->maxForce = 500000.0f; // emulate angular friction
return space;
}
CCSlideJoint::CCSlideJoint(CCPhysicsWorld *world,
CCPhysicsBody *bodyA, CCPhysicsBody *bodyB, cpVect archA, cpVect archB,
cpFloat min, cpFloat max) : CCJoint(world, bodyA, bodyB, SLIDE_JOINT)
{
this->m_constraint = cpSpaceAddConstraint(world->getSpace(),
cpSlideJointNew(bodyA->getBody(), bodyB->getBody(), archA, archB, min, max));
}
void weapon_update(Entity *ent) {
cpBody *body = entity_body(ent);
WeaponEntityData *data = entity_data(ent);
if (data->owner && !data->player_joint) {
cvar_setd_player(entity_owner(ent), "weapon_id", entity_id(ent));
data->player_joint = cpSpaceAddConstraint(game.space, cpPivotJointNew2(
body,
entity_body(data->owner),
cpv(0, -WEAPON_HEIGHT / 2), cpv(1, 0)
));
cpConstraintSetErrorBias(data->player_joint, 0);
}
int mult = 0;
if (keymap_is_held("mouse1")) mult++;
if (keymap_is_held("mouse2")) mult--;
if (mult) {
cpVect force_pos = cpv(0, WEAPON_HEIGHT);
cpVect weapon_pos = cpBodyGetPosition(body);
cpVect world_force_pos = cpvadd(weapon_pos, cpvrotate(cpBodyGetRotation(body), force_pos));
cpVect mouse_delta = cpvsub(keymap_mouse_world(), world_force_pos);
cpVect force = cpvmult(cpvnormalize(mouse_delta), mult * WEAPON_SWING_FORCE);
cpBodyApplyForceAtWorldPoint(body, force, world_force_pos);
}
}
void NaiveRotationAlgorithm::stepSystem(SystemInfo *individual)
{
MachineSystem *oldSystem = individual->system;
individual->system = new MachineSystem(*oldSystem); // copy it to stop all the damn bouncing about
delete oldSystem;
cpBody *inputBody = individual->system->partAtPosition(individual->system->inputMachinePosition)->body;
cpBody *outputBody = individual->system->partAtPosition(individual->system->outputMachinePosition)->body;
cpSpace *systemSpace = individual->system->getSpace();
cpConstraint *motor = cpSimpleMotorNew(cpSpaceGetStaticBody(systemSpace), inputBody, M_PI);
cpSpaceAddConstraint(systemSpace, motor);
cpConstraintSetMaxForce(motor, 50000);
for (int i=0; i<simSteps; i++) {
individual->inputValues[i] = (cpBodyGetAngle(inputBody));
cpSpaceStep(systemSpace, 0.1);
individual->outputValues[i] = (cpBodyGetAngle(outputBody));
}
cpSpaceRemoveConstraint(systemSpace, motor);
cpBodySetAngVel(outputBody, 0);
cpBodySetAngle(inputBody, 0);
}
static void
PostStepAddJoint(cpSpace *space, void *key, void *data)
{
// printf("Adding joint for %p\n", data);
cpConstraint *joint = (cpConstraint *)key;
cpSpaceAddConstraint(space, joint);
}
cpSpace* cpSpaceSerializer::load(cpSpace *space, const char* filename)
{
if (!_doc.LoadFile(filename))
return space;
//Grab our space
TiXmlElement *root = _doc.FirstChildElement("space");
if (!root)
return space;
_space = space;
//Initialize
_bodyMap.clear();
_shapeMap.clear();
//A body id of zero is the space's static body
_bodyMap[0] = space->staticBody;
space->iterations = createValue<int>("iterations", root);
space->gravity = createPoint("gravity", root);
space->damping = createValue<cpFloat>("damping", root);
TiXmlElement *child = root->FirstChildElement("shape");
//Read Shapes
while (child)
{
//attempt a shape
cpShape *shape = createShape(child);
if (shape)
{
//This should not happen like this, need to reflect reality -rkb
if (shape->body->m != INFINITY && !cpSpaceContainsBody(space, shape->body))
cpSpaceAddBody(space, shape->body);
cpSpaceAddShape(space, shape);
}
//Next!
child = child->NextSiblingElement("shape");
}
//Read Constraints
child = root->FirstChildElement("constraint");
while (child)
{
//else attempt a constraint
cpConstraint *constraint = createConstraint(child);
if (constraint)
cpSpaceAddConstraint(space, constraint);
child = child->NextSiblingElement("constraint");
}
return space;
}
static void
make_leg(cpSpace *space, cpFloat side, cpFloat offset, cpBody *chassis, cpBody *crank, cpVect anchor)
{
cpVect a, b;
cpShape *shape;
cpFloat leg_mass = 1.0f;
// make leg
a = cpvzero, b = cpv(0.0f, side);
cpBody *upper_leg = cpSpaceAddBody(space, cpBodyNew(leg_mass, cpMomentForSegment(leg_mass, a, b, 0.0f)));
cpBodySetPosition(upper_leg, cpv(offset, 0.0f));
shape = cpSpaceAddShape(space, cpSegmentShapeNew(upper_leg, a, b, seg_radius));
cpShapeSetFilter(shape, cpShapeFilterNew(1, CP_ALL_CATEGORIES, CP_ALL_CATEGORIES));
cpSpaceAddConstraint(space, cpPivotJointNew2(chassis, upper_leg, cpv(offset, 0.0f), cpvzero));
// lower leg
a = cpvzero, b = cpv(0.0f, -1.0f*side);
cpBody *lower_leg = cpSpaceAddBody(space, cpBodyNew(leg_mass, cpMomentForSegment(leg_mass, a, b, 0.0f)));
cpBodySetPosition(lower_leg, cpv(offset, -side));
shape = cpSpaceAddShape(space, cpSegmentShapeNew(lower_leg, a, b, seg_radius));
cpShapeSetFilter(shape, cpShapeFilterNew(1, CP_ALL_CATEGORIES, CP_ALL_CATEGORIES));
shape = cpSpaceAddShape(space, cpCircleShapeNew(lower_leg, seg_radius*2.0f, b));
cpShapeSetFilter(shape, cpShapeFilterNew(1, CP_ALL_CATEGORIES, CP_ALL_CATEGORIES));
cpShapeSetElasticity(shape, 0.0f);
cpShapeSetFriction(shape, 1.0f);
cpSpaceAddConstraint(space, cpPinJointNew(chassis, lower_leg, cpv(offset, 0.0f), cpvzero));
cpSpaceAddConstraint(space, cpGearJointNew(upper_leg, lower_leg, 0.0f, 1.0f));
cpConstraint *constraint;
cpFloat diag = cpfsqrt(side*side + offset*offset);
constraint = cpSpaceAddConstraint(space, cpPinJointNew(crank, upper_leg, anchor, cpv(0.0f, side)));
cpPinJointSetDist(constraint, diag);
constraint = cpSpaceAddConstraint(space, cpPinJointNew(crank, lower_leg, anchor, cpvzero));
cpPinJointSetDist(constraint, diag);
}
void CDynamics2DStretchableObjectModel::SetAngularFriction(Real f_max_bias,
Real f_max_force) {
m_ptAngularFriction =
cpSpaceAddConstraint(GetDynamics2DEngine().GetPhysicsSpace(),
cpGearJointNew(GetDynamics2DEngine().GetGroundBody(),
GetBody(),
0.0f,
1.0f));
m_ptAngularFriction->maxBias = f_max_bias;
m_ptAngularFriction->maxForce = f_max_force; // emulate angular friction
}
void CDynamics2DStretchableObjectModel::SetLinearFriction(Real f_max_bias,
Real f_max_force) {
m_ptLinearFriction =
cpSpaceAddConstraint(GetDynamics2DEngine().GetPhysicsSpace(),
cpPivotJointNew2(GetDynamics2DEngine().GetGroundBody(),
GetBody(),
cpvzero,
cpvzero));
m_ptLinearFriction->maxBias = f_max_bias;
m_ptLinearFriction->maxForce = f_max_force; // emulate linear friction
}
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;
}
WorldConstraint_t *worldConstr_createSimpleMotorJoint(WorldEntity_t *a, WorldEntity_t *b, GLMFloat aRate)
{
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_SimpleMotor;
ret->cpConstraint = cpSimpleMotorNew(a->cpBody, b->cpBody, aRate);
cpSpaceAddConstraint(ret->world->cpSpace, ret->cpConstraint);
return ret;
}
WorldConstraint_t *worldConstr_createRotaryLimitJoint(WorldEntity_t *a, WorldEntity_t *b, GLMFloat aMinAngle, GLMFloat aMaxAngle)
{
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_RotaryLimit;
ret->cpConstraint = cpRotaryLimitJointNew(a->cpBody, b->cpBody, aMinAngle, aMaxAngle);
cpSpaceAddConstraint(ret->world->cpSpace, ret->cpConstraint);
return ret;
}
WorldConstraint_t *worldConstr_createSlideJoint(WorldEntity_t *a, WorldEntity_t *b, vec2_t aAnchorA, vec2_t aAnchorB,
GLMFloat aMinDist, GLMFloat aMaxDist)
{
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_Slide;
ret->cpConstraint = cpSlideJointNew(a->cpBody, b->cpBody, VEC2_TO_CPV(aAnchorA), VEC2_TO_CPV(aAnchorB), aMinDist, aMaxDist);
cpSpaceAddConstraint(ret->world->cpSpace, ret->cpConstraint);
return ret;
}
WorldConstraint_t *worldConstr_createDampedSpringJoint(WorldEntity_t *a, WorldEntity_t *b, vec2_t aAnchorA, vec2_t aAnchorB,
GLMFloat aRestLength, GLMFloat aStiffness, GLMFloat aDamping)
{
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_DampedSpring;
ret->cpConstraint = cpDampedSpringNew(a->cpBody, b->cpBody, VEC2_TO_CPV(aAnchorA), VEC2_TO_CPV(aAnchorB), aRestLength, aStiffness, aDamping);
cpSpaceAddConstraint(ret->world->cpSpace, ret->cpConstraint);
return ret;
}
void PlayLayer::onBackwardDown() {
DoodleTruck *doodleTruck = DoodleTruck::sharedDoodleTruck();
if (!backward_touched){
if (!cpSpaceContainsConstraint(doodleTruck->getSpace(), (cpConstraint*)doodleTruck->getMotor()))
cpSpaceAddConstraint(doodleTruck->getSpace(), (cpConstraint*)doodleTruck->getMotor());
//playDriveSound();
}
backward_touched = true;
measureRPM->addRPM();
measureBoost->setBoost();
}
WorldConstraint_t *worldConstr_createDampedRotarySpringJoint(WorldEntity_t *a, WorldEntity_t *b,
GLMFloat aRestAngle, GLMFloat aStiffness, GLMFloat aDamping)
{
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_DampedRotarySpring;
ret->cpConstraint = cpDampedRotarySpringNew(a->cpBody, b->cpBody, aRestAngle, aStiffness, aDamping);
cpSpaceAddConstraint(ret->world->cpSpace, ret->cpConstraint);
return ret;
}
WorldConstraint_t *worldConstr_createGrooveJoint(WorldEntity_t *a, WorldEntity_t *b, vec2_t aGrooveStart, vec2_t aGrooveEnd,
vec2_t aAnchorB)
{
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_Groove;
ret->cpConstraint = cpGrooveJointNew(a->cpBody, b->cpBody, VEC2_TO_CPV(aGrooveStart), VEC2_TO_CPV(aGrooveEnd),
VEC2_TO_CPV(aAnchorB));
cpSpaceAddConstraint(ret->world->cpSpace, ret->cpConstraint);
return ret;
}
void physics_add_top_down_friction(cpBody *body, cpBody *control, float friction, cpConstraint **out_pivot, cpConstraint **out_gear) {
//emulates linear friction
cpConstraint *pivot = cpSpaceAddConstraint(game.space, cpPivotJointNew2(
control,
body,
cpvzero,
cpvzero
));
cpConstraintSetErrorBias(pivot, cpBodyGetMass(body) * friction);
cpConstraintSetMaxForce(pivot, cpBodyGetMass(body) * friction);
//emulates angular friction
cpConstraint *gear = cpSpaceAddConstraint(game.space, cpGearJointNew(
control,
body,
0, 1
));
cpConstraintSetMaxBias(gear, 0);
cpConstraintSetMaxForce(gear, friction / cpBodyGetMass(body) / 10);
if (out_pivot) *out_pivot = pivot;
if (out_gear) *out_gear = gear;
}
void DynamicObjectStabilizator::fixatePoints()
{
// vector<IDynamicObject *>::iterator begin = m_DynamicObjects.begin();
// vector<IDynamicObject *>::iterator end = m_DynamicObjects.end();
// vector<IDynamicObject *>::iterator iter = begin + 1;
// for( ; iter != end ; iter++ )
size_t count = m_DynamicObjects.size();
for( size_t object_i = 0 ; object_i < 2 ; object_i++ )
{
const IGeometryObject & geometryObjext = m_DynamicObjects[object_i]->getGeometryObject();
if( geometryObjext.getType() != GEOMETRYOBJECT_POINT )
{
// assert( false );
continue;
}
cpBody * kineticBody = cpBodyNewKinematic();
cpSpaceAddBody( m_Space, kineticBody );
m_KineticBodies.push_back( kineticBody );
const GeometryPoint & geometryPoint = dynamic_cast<const GeometryPoint &>( geometryObjext );
int x = geometryPoint.getX();
int y = geometryPoint.getY();
cpVect mousePoint = cpv( x, y );
cpShape * shape = cpSpacePointQueryNearest( m_Space, mousePoint, 100.0, GRAB_FILTER, 0 );
if( 0 == shape )
{
return;
}
cpVect new_mouse_position = cpv( x, y );
cpBodySetPosition( kineticBody, new_mouse_position );
cpBody * trackingBody = cpShapeGetBody( shape );
cpConstraint * joint = cpPivotJointNew2( kineticBody, trackingBody, cpvzero, cpvzero );
cpSpaceAddConstraint( m_Space, joint );
m_Joints.push_back( joint );
break; //one pointb
}
}
void GeometrySpringDynamic::initSpring()
{
GeometrySpringGetAngles getAngles( this );
// int linkFromAbsoluteAngle = getAngles.getLinkFromAngle();
// int linkToAbsoluteAngle = getAngles.getLinkToAngle();
// float angle = M_PI / 180.0 * angleInt;
const GeometryLinkDynamic * linkFrom = getDynamicLinkFrom();
const GeometryLinkDynamic * linkTo = getDynamicLinkTo();
cpBody * bodyFrom = linkFrom->getBody();
cpBody * bodyTo = linkTo->getBody();
GetRotateAngleFunctor getAngle( this );
int rotateAngleInt = getAngle.getAngle();
float rotateAngle = M_PI / 180.0 * rotateAngleInt;
m_ConstraintGear = cpSpaceAddConstraint( m_Space, cpGearJointNew( bodyFrom, bodyTo, rotateAngle, 1.0f ) );
}
bool PhysicsJoint::initJoint()
{
bool ret = !_initDirty;
while (_initDirty)
{
ret = createConstraints();
CC_BREAK_IF(!ret);
for (auto subjoint : _cpConstraints)
{
subjoint->maxForce = _maxForce;
subjoint->errorBias = cpfpow(1.0f - 0.15f, 60.0f);
cpSpaceAddConstraint(_world->_cpSpace, subjoint);
}
_initDirty = false;
ret = true;
}
return ret;
}
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;
}
static cpSpace *
init(void)
{
cpResetShapeIdCounter();
space = cpSpaceNew();
cpSpaceResizeActiveHash(space, 30.0f, 1000);
space->iterations = 10;
cpShape *shape;
// Create segments around the edge of the screen.
shape = cpSpaceAddStaticShape(space, cpSegmentShapeNew(NULL, cpv(-320,-240), cpv(-320,240), 0.0f));
shape->e = 1.0f; shape->u = 1.0f;
shape->layers = NOT_GRABABLE_MASK;
shape = cpSpaceAddStaticShape(space, cpSegmentShapeNew(NULL, cpv(320,-240), cpv(320,240), 0.0f));
shape->e = 1.0f; shape->u = 1.0f;
shape->layers = NOT_GRABABLE_MASK;
shape = cpSpaceAddStaticShape(space, cpSegmentShapeNew(NULL, cpv(-320,-240), cpv(320,-240), 0.0f));
shape->e = 1.0f; shape->u = 1.0f;
shape->layers = NOT_GRABABLE_MASK;
shape = cpSpaceAddStaticShape(space, cpSegmentShapeNew(NULL, cpv(-320,240), cpv(320,240), 0.0f));
shape->e = 1.0f; shape->u = 1.0f;
shape->layers = NOT_GRABABLE_MASK;
for(int i=0; i<10; i++)
add_box();
cpBody *body = cpSpaceAddBody(space, cpBodyNew(100.0f, 10000.0f));
shape = cpSpaceAddShape(space, cpSegmentShapeNew(body, cpv(-75,0), cpv(75,0), 5.0f));
shape->e = 1.0f; shape->u = 1.0f;
cpSpaceAddConstraint(space, cpPivotJointNew2(body, NULL, cpvzero, cpvzero));
return space;
}
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;
}
static void
click(int button, int state, int x, int y)
{
if(button == GLUT_LEFT_BUTTON){
if(state == GLUT_DOWN){
cpVect point = mouseToSpace(x, y);
cpShape *shape = cpSpacePointQueryFirst(space, point, GRABABLE_MASK_BIT, CP_NO_GROUP);
if(shape){
cpBody *body = shape->body;
mouseJoint = cpPivotJointNew2(mouseBody, body, cpvzero, cpBodyWorld2Local(body, point));
mouseJoint->maxForce = 50000.0f;
mouseJoint->errorBias = cpfpow(1.0f - 0.15f, 60.0f);
cpSpaceAddConstraint(space, mouseJoint);
}
} else if(mouseJoint){
cpSpaceRemoveConstraint(space, mouseJoint);
cpConstraintFree(mouseJoint);
mouseJoint = NULL;
}
}
}
/* Mouse handling is a bit tricky. We want the user to move
* tiles using the mouse but because tiles are dynamic bodies
* managed by Chipmunk2D, we cannot directly control them.
* This is resolved by creating a pivot joint between an
* invisible mouse body that we can control and the tile body
* that we cannot directly control.
*/
static void apply_mouse_motion(struct state* state)
{
struct mouse m;
update_mouse(&m);
int w, h;
get_screen_size(&w, &h);
int x = m.x_position * w;
int y = m.y_position * h;
cpVect mouse_pos = cpv(x, y);
cpVect new_point =
cpvlerp(cpBodyGetPosition(state->mouse_body), mouse_pos, 0.25f);
cpBodySetVelocity(
state->mouse_body,
cpvmult(cpvsub(new_point, cpBodyGetPosition(state->mouse_body)),
60.0f));
cpBodySetPosition(state->mouse_body, new_point);
if (m.left_click && state->mouse_joint == NULL) {
cpFloat radius = 5.0;
cpPointQueryInfo info = { 0 };
cpShape* shape = cpSpacePointQueryNearest(state->space, mouse_pos,
radius, GRAB_FILTER, &info);
if (shape && cpBodyGetMass(cpShapeGetBody(shape)) < INFINITY) {
cpVect nearest = (info.distance > 0.0f ? info.point : mouse_pos);
cpBody* body = cpShapeGetBody(shape);
state->mouse_joint =
cpPivotJointNew2(state->mouse_body, body, cpvzero,
cpBodyWorldToLocal(body, nearest));
cpConstraintSetMaxForce(state->mouse_joint, 5000000.0f);
cpConstraintSetErrorBias(state->mouse_joint,
cpfpow(1.0f - 0.15f, 60.0f));
cpSpaceAddConstraint(state->space, state->mouse_joint);
}
}
if (m.left_click == false && state->mouse_joint != NULL) {
cpSpaceRemoveConstraint(state->space, state->mouse_joint);
cpConstraintFree(state->mouse_joint);
state->mouse_joint = NULL;
}
}
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)));
}
CDynamics2DCylinderEntity::CDynamics2DCylinderEntity(CDynamics2DEngine& c_engine,
CCylinderEntity& c_entity) :
CDynamics2DEntity(c_engine, c_entity.GetEmbodiedEntity()),
m_cCylinderEntity(c_entity),
m_fMass(c_entity.GetMass()),
m_ptShape(NULL),
m_ptBody(NULL) {
/* Get the radius of the entity */
Real fRadius = c_entity.GetRadius();
m_fHalfHeight = c_entity.GetHeight() * 0.5f;
/* Create a circle object in the physics space */
const CVector3& cPosition = GetEmbodiedEntity().GetPosition();
if(c_entity.IsMovable()) {
/* The cylinder is movable */
/* Create the body */
m_ptBody = cpSpaceAddBody(m_cEngine.GetPhysicsSpace(),
cpBodyNew(m_fMass,
cpMomentForCircle(m_fMass,
0,
fRadius + fRadius,
cpvzero)));
m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
CRadians cXAngle, cYAngle, cZAngle;
GetEmbodiedEntity().GetOrientation().ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpBodySetAngle(m_ptBody, cZAngle.GetValue());
/* Create the geometry */
m_ptShape = cpSpaceAddShape(m_cEngine.GetPhysicsSpace(),
cpCircleShapeNew(m_ptBody, fRadius, cpvzero));
/* This object is grippable */
m_ptShape->collision_type = CDynamics2DEngine::SHAPE_GRIPPABLE;
m_ptShape->data = reinterpret_cast<void*>(&c_entity);
/* No elasticity */
m_ptShape->e = 0.0;
/* Lots surface contact friction to help pushing */
m_ptShape->u = 0.7;
/* Friction with ground */
m_ptLinearFriction =
cpSpaceAddConstraint(m_cEngine.GetPhysicsSpace(),
cpPivotJointNew2(m_cEngine.GetGroundBody(),
m_ptBody,
cpvzero,
cpvzero));
m_ptLinearFriction->biasCoef = 0.0f; // disable joint correction
m_ptLinearFriction->maxForce = 1.0f; // emulate linear friction
m_ptAngularFriction =
cpSpaceAddConstraint(m_cEngine.GetPhysicsSpace(),
cpGearJointNew(m_cEngine.GetGroundBody(),
m_ptBody,
0.0f,
1.0f));
m_ptAngularFriction->biasCoef = 0.0f; // disable joint correction
m_ptAngularFriction->maxForce = 5.0f; // emulate angular friction
}
else {
/* The cylinder is not movable */
/* Create the geometry */
m_ptShape = cpSpaceAddStaticShape(m_cEngine.GetPhysicsSpace(),
cpCircleShapeNew(m_cEngine.GetGroundBody(),
fRadius,
cpv(cPosition.GetX(), cPosition.GetY())));
/* This object is normal */
m_ptShape->collision_type = CDynamics2DEngine::SHAPE_NORMAL;
m_ptShape->data = reinterpret_cast<void*>(&c_entity);
/* No elasticity */
m_ptShape->e = 0.0;
/* Little contact friction to help sliding away */
m_ptShape->u = 0.1;
}
}
