ETERM *body_update_position(ETERM *fromp, ETERM *argp) {
// get the args
ETERM *space_refp = erl_element(1, argp);
ETERM *idp = erl_element(2, argp);
ETERM *deltap = erl_element(3, argp);
erlmunk_space *s = NULL;
int space_id = ERL_REF_NUMBER(space_refp);
HASH_FIND_INT(erlmunk_spaces, &space_id, s);
int body_id = ERL_INT_VALUE(idp);
erlmunk_body *b = NULL;
HASH_FIND_INT(s->bodies, &body_id, b);
if (b == NULL)
return NULL;
cpVect position = cpBodyGetPosition(b->body);
float angle = deg_to_rad(cpBodyGetAngle(b->body));
cpVect angleV = cpvforangle(angle);
cpVect projection = cpvmult(angleV, ERL_FLOAT_VALUE(deltap));
cpVect new_position = cpvadd(projection, position);
cpBodySetPosition(b->body, new_position);
// DEBUGF(("body_update_position(x: %f, y: %f, delta: %f) has succeeded (x: %f, y: %f)",
// position.x, position.y, ERL_FLOAT_VALUE(deltap),
// new_position.x, new_position.y));
return NULL;
}
void GameTrack::updateCarSensors(GameCar* car) {
if(!car->getSensorEnabled()) return;
float* data=car->getSensorData();
float spread=car->getSensorSpread();
int pixels=car->getSensorPixels();
float len=car->getSensorLength();
cpSpace* space=sim->get_space();
cpBody* body=car->physic_object->get_body();
for(int i=0;i<pixels;i++) {
float angle=car->getSensorAngle(i);
float y_offset=0.8; //fabs(angle)/45;
cpVect start={0,y_offset};
cpVect end=cpvmult(cpvforangle(degToRad(angle)),len);
start=cpBodyLocal2World(body,start);
end=cpBodyLocal2World(body,end);
cpSegmentQueryInfo info;
cpSpaceSegmentQueryFirst(space,start,end,car->getLayers(),CP_NO_GROUP,&info);
data[i]=info.t;
}
}
ETERM *body_apply_impulse(ETERM *fromp, ETERM *argp) {
// get the args
ETERM *space_refp = erl_element(1, argp);
ETERM *idp = erl_element(2, argp);
ETERM *impulsep = erl_element(3, argp);
erlmunk_space *s;
int space_id = ERL_REF_NUMBER(space_refp);
HASH_FIND_INT(erlmunk_spaces, &space_id, s);
int body_id = ERL_INT_VALUE(idp);
erlmunk_body *b;
HASH_FIND_INT(s->bodies, &body_id, b);
if (b == NULL)
return NULL;
// apply the impulse at the center of the body and along it's current angle
float angle = deg_to_rad(cpBodyGetAngle(b->body));
cpVect angleV = cpvforangle(angle);
cpVect impulse = cpvmult(angleV, ERL_FLOAT_VALUE(impulsep));
cpBodyApplyImpulseAtWorldPoint(b->body, impulse, angleV);
return NULL;
}
void update_drive()
{
const cpFloat max_forward_speed = 150;
const cpFloat max_backward_speed = -20;
const cpFloat max_drive_force = 100;
int i;
for(i=0; i<1; i++) {
cpFloat desired_speed = 0;
// find desired speed
if(controls.forward)
desired_speed = max_forward_speed;
else if(controls.back)
desired_speed = max_backward_speed;
// find speed
cpVect forward_normal = cpvperp(cpvforangle(cpBodyGetAngle(tire[i])));
cpFloat speed = cpvdot(forward_velocity(i), forward_normal);
// apply force
cpFloat force = 0;
if(desired_speed > speed)
force = max_drive_force;
else if(desired_speed < speed)
force = -max_drive_force;
else
return;
cpBodyApplyImpulse(tire[i], cpvmult(forward_normal, force), cpvzero);
}
}
void draw_shape(cpBody* body, cpShape* shape, void* data)
{
// get body info
cpVect v = cpBodyGetPos(body);
cpFloat angle = cpBodyGetAngle(body);
cpVect rot = cpvforangle(angle);
// get vectors
int n = cpPolyShapeGetNumVerts(shape);
SDL_Point* pts = calloc(sizeof(SDL_Point), n+1);
// rotate vectors
int i;
for(i=0; i<n; i++) {
cpVect p = cpPolyShapeGetVert(shape, i);
cpVect vr = cpvrotate(cpv(p.x,p.y), rot);
pts[i] = (SDL_Point) { (vr.x+v.x)*10+50, (vr.y+v.y)*10+50 };
if(i == 0)
pts[n] = pts[i];
}
// draw
SDL_RenderDrawLines(ren, pts, n+1);
free(pts);
}
// returns the transform matrix according the Chipmunk Body values
CCAffineTransform CCPhysicsSprite::nodeToParentTransform()
{
cpVect rot = (m_bIgnoreBodyRotation ? cpvforangle(-CC_DEGREES_TO_RADIANS(m_fRotationX)) : m_pCPBody->rot);
float x = m_pCPBody->p.x + rot.x*(-m_obAnchorPointInPoints.x) - rot.y*(-m_obAnchorPointInPoints.y);
float y = m_pCPBody->p.y + rot.y*(-m_obAnchorPointInPoints.x) + rot.x*(-m_obAnchorPointInPoints.y);
if (m_bIgnoreAnchorPointForPosition)
{
x += m_obAnchorPointInPoints.x;
y += m_obAnchorPointInPoints.y;
}
return (m_sTransform = CCAffineTransformMake(rot.x, rot.y, -rot.y, rot.x, x, y));
}
CCAffineTransform cpCCNode::nodeToParentTransform ( KDvoid )
{
cpBody* pBody = m_pImplementation->getBody ( );
cpVect tRot = m_pImplementation->isIgnoreRotation ( ) ? cpvforangle ( -CC_DEGREES_TO_RADIANS ( m_tRotation.x ) ) : pBody->rot;
KDfloat x = pBody->p.x + tRot.x * -m_tAnchorPointInPoints.x - tRot.y * - m_tAnchorPointInPoints.y;
KDfloat y = pBody->p.y + tRot.y * -m_tAnchorPointInPoints.x + tRot.x * - m_tAnchorPointInPoints.y;
if ( m_pImplementation->isIgnoreRotation ( ) )
{
x += m_tAnchorPointInPoints.x;
y += m_tAnchorPointInPoints.y;
}
m_tTransform = CCAffineTransformMake ( tRot.x, tRot.y, -tRot.y, tRot.x, x, y );
return m_tTransform;
}
// returns the transform matrix according the Chipmunk Body values
CCAffineTransform CCPhysicsSprite::nodeToParentTransform()
{
// Although scale is not used by physics engines, it is calculated just in case
// the sprite is animated (scaled up/down) using actions.
// For more info see: http://www.cocos2d-iphone.org/forum/topic/68990
cpVect rot = (m_bIgnoreBodyRotation ? cpvforangle(-CC_DEGREES_TO_RADIANS(m_fRotationX)) : m_pCPBody->rot);
float x = m_pCPBody->p.x + rot.x * -m_obAnchorPointInPoints.x * m_fScaleX - rot.y * -m_obAnchorPointInPoints.y * m_fScaleY;
float y = m_pCPBody->p.y + rot.y * -m_obAnchorPointInPoints.x * m_fScaleX + rot.x * -m_obAnchorPointInPoints.y * m_fScaleY;
if (m_bIgnoreAnchorPointForPosition)
{
x += m_obAnchorPointInPoints.x;
y += m_obAnchorPointInPoints.y;
}
return (m_sTransform = CCAffineTransformMake(rot.x * m_fScaleX, rot.y * m_fScaleX,
-rot.y * m_fScaleY, rot.x * m_fScaleY,
x, y));
}
void Bomb::explode(int timestep) {
exploding = true;
cpVect blast_dir;
for (int j=0; j < PARTICLE_TOTAL; j++) {
if (particles[j]->dead) {
blast_dir = cpvmult(cpvforangle(DEG2RAD(j*3)), (rand()%100)+40);
particles[j]->x = x;
particles[j]->y = y;
particles[j]->x_speed = blast_dir.x;
particles[j]->y_speed = blast_dir.y;
particles[j]->ttl = 1250;
particles[j]->birth = timestep;
particles[j]->color = 100;
particles[j]->dead = false;
}
}
}
void
cpBodySetAngle(cpBody *body, cpFloat a)
{
body->a = fmod(a, (cpFloat)M_PI*2.0f);
body->rot = cpvforangle(a);
}
static void update(void) {
int steps, i;
cpFloat dt;
cpVect look, lastPos, curPos;
projectile_t *pj; /* temp for looping */
static cpVect delta = {0, 0};
static projectile_t *curProj = NULL;
static bool canToggleEditor = true;
curPos = atlMousePos();
lastPos = atlMouseLastPos();
/* rotate 'gun' based on mouse position */
look = cpvnormalize(cpvsub(curPos, g_Cannon->body->p));
cpBodySetAngle(g_Cannon->body, cpvtoangle(look));
if (isKeyPressed(KEY_e) && canToggleEditor) {
canToggleEditor = false;
editorMode = !editorMode;
} else if (!isKeyPressed(KEY_e)) {
canToggleEditor = true;
}
if (editorMode) {
if (!editorBody)
initializeEditor();
handleEditor();
} else {
if (editorBody)
destroyEditor();
if (atlLeftMouseDown()) {
if (!curProj) {
/* don't let player hold the left mouse button to fire multiple rounds */
delta = cpvnormalize(cpvsub(atlMouseClickPos(), g_Cannon->body->p));
if (g_Cannon->ai < MAX_PROJECTILES) {
curProj = g_Cannon->ammo[g_Cannon->ai++];
if (curProj) {
cpSpaceAddBody(g_Space, curProj->body);
curProj->body->p = cpvadd(g_Cannon->body->p,
cpvmult(look, g_Cannon->length));
cpSpaceAddShape(g_Space, curProj->shape);
cpBodyApplyImpulse(curProj->body,
cpvmult(cpvforangle(g_Cannon->body->a), 600.0f),
cpvzero);
cpBodyActivate(curProj->body);
}
}
}
} else {
curProj = NULL;
}
}
/* treat projectiles as limited resources. only allocated at the start of a level */
for (i = 0; i <= g_Cannon->ai-1; ++i) {
pj = g_Cannon->ammo[i];
if (!pj)
continue;
if (pj->body->v.x >= -0.01f && pj->body->v.x <= 0.01f &&
pj->body->v.y >= -0.01f && pj->body->v.y <= 0.01f) {
/* TODO: mark an object for deletion, but don't delete immediately */
cpSpaceRemoveBody(g_Space, pj->body);
cpSpaceRemoveShape(g_Space, pj->shape);
free(g_Cannon->ammo[i]);
g_Cannon->ammo[i] = NULL;
}
}
steps = 3;
dt = 1.0f/60.0f/(cpFloat)steps;
for (i = 0; i < steps; ++i) {
cpSpaceStep(g_Space, dt);
}
}
static void
draw_shape(cpShape* shape, SDL_Surface* screen)
{
cpBody* body = shape->body;
struct draw_options opts = {
.surface = screen,
.colour = colour(200, 200, 200)
};
switch (shape->CP_PRIVATE(klass)->type)
{
case CP_CIRCLE_SHAPE:
draw_circle_shape(opts, shape, body);
break;
case CP_SEGMENT_SHAPE:
draw_segment_shape(opts, shape, body);
break;
case CP_POLY_SHAPE:
draw_poly_shape(opts, shape, body);
break;
default:
debug_putsf("ignoring unrecognised shape type %d",
shape->CP_PRIVATE(klass)->type);
break;
}
}
static void
draw_constraint(cpConstraint* constraint, SDL_Surface *screen)
{
struct draw_options opts = {
.surface = screen,
.colour = colour(100, 100, 200)
};
cpVect vect_a = cpBodyGetPos(cpConstraintGetA(constraint));
cpVect vect_b = cpBodyGetPos(cpConstraintGetB(constraint));
draw_line(opts, vect_a, vect_b);
}
static void
draw_rotation_vector(cpBody *body, SDL_Surface *screen)
{
struct draw_options opts = {
.surface = screen,
.colour = colour(200, 100, 100)
};
cpFloat rotation_vector_length = 30;
cpVect pos = cpBodyGetPos(body);
cpVect rotation = cpvmult(cpvforangle(cpBodyGetAngle(body)),
rotation_vector_length);
draw_line(opts, pos, cpvadd(pos, rotation));
}
static void
draw_forces(cpBody *body, SDL_Surface *screen)
{
struct draw_options opts = {
.surface = screen,
.colour = colour(100, 200, 100)
};
cpVect pos = cpBodyGetPos(body);
cpVect force = cpBodyGetForce(body);
draw_line(opts, pos, cpvadd(pos, force));
}
static void
draw_body(cpBody *body, SDL_Surface *screen)
{
draw_rotation_vector(body, screen);
draw_forces(body, screen);
}
void
debug_draw_space(cpSpace* space, SDL_Surface* screen)
{
cpSpaceEachShape(space,
(cpSpaceShapeIteratorFunc)draw_shape,
screen);
cpSpaceEachConstraint(space,
(cpSpaceConstraintIteratorFunc)draw_constraint,
screen);
cpSpaceEachBody(space,
(cpSpaceBodyIteratorFunc)draw_body,
screen);
}
cpVect cpvrotate(cpVect v, float a)
{
return cpvrotate(v, cpvforangle(a));
}
static cpSpace *
init(void)
{
ChipmunkDemoMessageString = "Use the arrow keys to control the machine.";
cpSpace *space = cpSpaceNew();
cpSpaceSetIterations(space, 20);
cpSpaceSetGravity(space, cpv(0,-500));
cpBody *staticBody = cpSpaceGetStaticBody(space);
cpShape *shape;
cpVect a, b;
// Create segments around the edge of the screen.
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(-320,240), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(320,-240), cpv(320,240), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(320,-240), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
cpFloat offset = 30.0f;
// make chassis
cpFloat chassis_mass = 2.0f;
a = cpv(-offset, 0.0f), b = cpv(offset, 0.0f);
cpBody *chassis = cpSpaceAddBody(space, cpBodyNew(chassis_mass, cpMomentForSegment(chassis_mass, a, b, 0.0f)));
shape = cpSpaceAddShape(space, cpSegmentShapeNew(chassis, a, b, seg_radius));
cpShapeSetFilter(shape, cpShapeFilterNew(1, CP_ALL_CATEGORIES, CP_ALL_CATEGORIES));
// make crank
cpFloat crank_mass = 1.0f;
cpFloat crank_radius = 13.0f;
cpBody *crank = cpSpaceAddBody(space, cpBodyNew(crank_mass, cpMomentForCircle(crank_mass, crank_radius, 0.0f, cpvzero)));
shape = cpSpaceAddShape(space, cpCircleShapeNew(crank, crank_radius, cpvzero));
cpShapeSetFilter(shape, cpShapeFilterNew(1, CP_ALL_CATEGORIES, CP_ALL_CATEGORIES));
cpSpaceAddConstraint(space, cpPivotJointNew2(chassis, crank, cpvzero, cpvzero));
cpFloat side = 30.0f;
int num_legs = 2;
for(int i=0; i<num_legs; i++){
make_leg(space, side, offset, chassis, crank, cpvmult(cpvforangle((cpFloat)(2*i+0)/(cpFloat)num_legs*M_PI), crank_radius));
make_leg(space, side, -offset, chassis, crank, cpvmult(cpvforangle((cpFloat)(2*i+1)/(cpFloat)num_legs*M_PI), crank_radius));
}
motor = cpSpaceAddConstraint(space, cpSimpleMotorNew(chassis, crank, 6.0f));
return space;
}
cpVect forward_velocity(int n)
{
cpVect normal = cpvperp(cpvforangle(cpBodyGetAngle(tire[n])));
return cpvmult(normal, cpvdot(normal, cpBodyGetVel(tire[n])));
}
cpVect lateral_velocity(int n)
{
cpVect normal = cpvforangle(cpBodyGetAngle(tire[n]));
return cpvmult(normal, cpvdot(normal, cpBodyGetVel(tire[n])));
}
CDynamics2DBoxEntity::CDynamics2DBoxEntity(CDynamics2DEngine& c_engine,
CBoxEntity& c_entity) :
CDynamics2DEntity(c_engine, c_entity.GetEmbodiedEntity()),
m_cBoxEntity(c_entity),
m_fMass(c_entity.GetMass()),
m_ptShape(NULL),
m_ptBody(NULL) {
/* Get the size of the entity */
CVector3 cHalfSize = c_entity.GetSize() * 0.5f;
m_fHalfHeight = cHalfSize.GetZ();
/* Create a polygonal object in the physics space */
/* Start defining the vertices
NOTE: points must be defined in a clockwise winding
*/
cpVect tVertices[] = {
cpv(-cHalfSize.GetX(), -cHalfSize.GetY()),
cpv(-cHalfSize.GetX(), cHalfSize.GetY()),
cpv( cHalfSize.GetX(), cHalfSize.GetY()),
cpv( cHalfSize.GetX(), -cHalfSize.GetY())
};
const CVector3& cPosition = GetEmbodiedEntity().GetPosition();
CRadians cXAngle, cYAngle, cZAngle;
GetEmbodiedEntity().GetOrientation().ToEulerAngles(cZAngle, cYAngle, cXAngle);
if(c_entity.GetEmbodiedEntity().IsMovable()) {
/* The box is movable */
/* Create the body */
m_ptBody =
cpSpaceAddBody(m_cEngine.GetPhysicsSpace(),
cpBodyNew(m_fMass,
cpMomentForPoly(m_fMass,
4,
tVertices,
cpvzero)));
m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
cpBodySetAngle(m_ptBody, cZAngle.GetValue());
/* Create the geometry */
m_ptShape =
cpSpaceAddShape(m_cEngine.GetPhysicsSpace(),
cpPolyShapeNew(m_ptBody,
4,
tVertices,
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 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->maxBias = 0.0f; // disable joint correction
m_ptLinearFriction->maxForce = 1.49f; // emulate linear friction (this is just slightly smaller than FOOTBOT_MAX_FORCE)
m_ptAngularFriction =
cpSpaceAddConstraint(m_cEngine.GetPhysicsSpace(),
cpGearJointNew(m_cEngine.GetGroundBody(),
m_ptBody,
0.0f,
1.0f));
m_ptAngularFriction->maxBias = 0.0f; // disable joint correction
m_ptAngularFriction->maxForce = 1.49f; // emulate angular friction (this is just slightly smaller than FOOTBOT_MAX_TORQUE)
}
else {
/* The box is not movable */
/* Manually rotate the vertices */
cpVect tRot = cpvforangle(cZAngle.GetValue());
tVertices[0] = cpvrotate(tVertices[0], tRot);
tVertices[1] = cpvrotate(tVertices[1], tRot);
tVertices[2] = cpvrotate(tVertices[2], tRot);
tVertices[3] = cpvrotate(tVertices[3], tRot);
/* Create the geometry */
m_ptShape =
cpSpaceAddStaticShape(m_cEngine.GetPhysicsSpace(),
cpPolyShapeNew(m_cEngine.GetGroundBody(),
4,
tVertices,
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;
}
}
static inline void
setAngle(cpBody *body, cpFloat angle)
{
body->a = angle;//fmod(a, (cpFloat)M_PI*2.0f);
body->rot = cpvforangle(angle);
}
static int cpVect_forangle(lua_State *L) {
cpFloat a = (cpFloat)luaL_checknumber(L, 1);
push_cpVect(L, cpvforangle(a));
return 2;
}
void landerUpdate(Lander l)
{
cpVect force = cpvmult(cpvforangle(radiansFromOrientation(l->orientation)), l->thrust);
cpBodyApplyImpulse(l->body, force, cpvzero);
}
