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);
}
float PhysicsBody::getRotation()
{
if (_recordedAngle != cpBodyGetAngle(_cpBody)) {
_recordedAngle = cpBodyGetAngle(_cpBody);
_recordedRotation = - _recordedAngle * 180.0 / M_PI - _rotationOffset;
}
return _recordedRotation;
}
//Hakee yhden muuttujan uudet arvot
__declspec( dllexport ) void pull( const void * _in, int in_size, void * _out, int out_sz )
{
int index;
Variable *var;
cpBody *body;
cpShape *shape;
index = PEEKINT(INPUT_MEMBLOCK,0);
var = vhGetVariable(&mVariableHandler,index);
switch (var->mType)
{
case VarTypeBody:
body = (cpBody*)var->mPtr;
POKEFLOAT(OUTPUT_MEMBLOCK,0,radToDeg(cpBodyGetAngle(body)));
POKEVECT(OUTPUT_MEMBLOCK,4,cpBodyGetPos(body));
POKEFLOAT(OUTPUT_MEMBLOCK,12,radToDeg(cpBodyGetAngVel(body)));
POKEVECT(OUTPUT_MEMBLOCK,16,cpBodyGetVel(body));
break;
case VarTypeShape:
shape = (cpShape*)var->mPtr;
POKEINT(OUTPUT_MEMBLOCK,0,VarTypeShape);
default:
MessageBoxA(NULL,"cpPull: Invalid variable type","cpChipmunk error",MB_OK);
exit(0);
break;
}
}
void LinkerAgent::UpdateFromChipmunk()
{
float angle = (float)cpBodyGetAngle(m_pBody);
cpVect cpPosition = cpBodyGetPos(m_pBody);
m_pSprite->SetPosition((float)cpPosition.x, (float)-cpPosition.y);
m_pSprite->SetRotation(angle);
}
__declspec( dllexport ) void pullall( const void * _in, int in_size, void * _out, int out_sz )
{
int i;
cpBody *body;
Variable *var;
//int bodyCount = 0;
int index = 0;
for (i = 0;i != mVariableHandler.mSize;i++)
{
var = mVariableHandler.mPtrArray[i];
if (var == NULL)continue;
//Varmistetaan, että haetaan vain runkojen tiedot
if (var->mType != VarTypeBody)continue;
body = (cpBody*)var->mPtr;
//Eikä staattisen tietoja
if (cpBodyIsStatic(body))continue;
//Alkuun cb:n muistipalan osoite
POKEINT(OUTPUT_MEMBLOCK,index,var->mCBPtr);
POKEFLOAT(OUTPUT_MEMBLOCK,index+4,radToDeg(cpBodyGetAngle(body)));
POKEVECT(OUTPUT_MEMBLOCK,index+8,cpBodyGetPos(body));
POKEFLOAT(OUTPUT_MEMBLOCK,index+16,radToDeg(cpBodyGetAngVel(body)));
POKEVECT(OUTPUT_MEMBLOCK,index+20,cpBodyGetVel(body));
index += 28;
//bodyCount++;
}
//POKEINT(OUTPUT_MEMBLOCK,0,bodyCount);
}
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 physics_add(Entity ent)
{
PhysicsInfo *info;
if (entitypool_get(pool, ent))
return; /* already has physics */
transform_add(ent);
info = entitypool_add(pool, ent);
info->mass = 1.0;
info->type = PB_DYNAMIC;
/* create, init cpBody */
info->body = cpSpaceAddBody(space, cpBodyNew(info->mass, 1.0));
cpBodySetUserData(info->body, ent); /* for cpBody -> Entity mapping */
cpBodySetPos(info->body, cpv_of_vec2(transform_get_position(ent)));
cpBodySetAngle(info->body, transform_get_rotation(ent));
info->last_dirty_count = transform_get_dirty_count(ent);
/* initially no shapes */
info->shapes = array_new(ShapeInfo);
/* initialize last_pos/last_ang info for kinematic bodies */
info->last_pos = cpBodyGetPos(info->body);
info->last_ang = cpBodyGetAngle(info->body);
info->collisions = NULL;
}
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;
}
static void _set_type(PhysicsInfo *info, PhysicsBody type)
{
if (info->type == type)
return; /* already set */
info->type = type;
switch (type)
{
case PB_KINEMATIC:
info->last_pos = cpBodyGetPos(info->body);
info->last_ang = cpBodyGetAngle(info->body);
/* fall through */
case PB_STATIC:
if (!cpBodyIsStatic(info->body))
{
cpSpaceRemoveBody(space, info->body);
cpSpaceConvertBodyToStatic(space, info->body);
}
break;
case PB_DYNAMIC:
cpSpaceConvertBodyToDynamic(space, info->body, info->mass, 1.0);
cpSpaceAddBody(space, info->body);
_recalculate_moment(info);
break;
}
}
ETERM *body_copy(ETERM *fromp, ETERM *argp) {
// get the args
ETERM *space_refp = erl_element(1, argp);
ETERM *idp = erl_element(2, argp);
ETERM *from_idp = 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;
int from_body_id = ERL_INT_VALUE(from_idp);
erlmunk_body *from_b;
HASH_FIND_INT(s->bodies, &from_body_id, from_b);
// DEBUGF(("copying location from body #%d(%p) to #%d(%p)",
// from_body_id, from_b, body_id, b));
// copy position and angle from the from body
cpBodySetPosition(b->body, cpBodyGetPosition(from_b->body));
cpBodySetAngle(b->body, cpBodyGetAngle(from_b->body));
cpBodySetVelocity(b->body, cpBodyGetVelocity(from_b->body));
return NULL;
}
void handle_subscriber(erlmunk_subscriber *subscriber, element *bodies) {
element *el;
int count = 0;
LL_COUNT(bodies, el, count);
// DEBUGF(("# bodies in subscriber bounding box: %d", count));
ETERM **l_array = (ETERM **) malloc(sizeof(ETERM) * count);
int nth_body = 0;
LL_FOREACH(bodies, el) {
cpVect vect = cpBodyGetPosition(el->body);
float angle = cpBodyGetAngle(el->body);
// cpVect vel = cpBodyGetVelocity(el->body);
erlmunk_body_data *data = (erlmunk_body_data *) cpBodyGetUserData(el->body);
// DEBUGF(("id: %d, x: %f, y: %f, angle: %f, vel.x: %f, vel.y: %f, data: %p",
// data->id, vect.x, vect.y, angle, vel.x, vel.y, data));
ETERM **t_array = (ETERM **) malloc(sizeof(ETERM) * 4);
t_array[0] = erl_mk_float(vect.x);
t_array[1] = erl_mk_float(vect.y);
t_array[2] = erl_mk_float(angle);
if (data->term == NULL)
t_array[3] = erl_mk_undefined();
else
t_array[3] = erl_copy_term(data->term);
ETERM *tuple = erl_mk_tuple(t_array, 4);
free(t_array);
ETERM *prop_value = erl_mk_int_prop_value(data->id, tuple);
l_array[nth_body++] = prop_value;
}
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);
}
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);
}
}
float RCPBody::angle() const
{
if (mType != Static && mBody) {
mAngle = radToDeg(cpBodyGetAngle(mBody));
}
return mAngle;
}
static int l_physics_getBodyAngle(lua_State* state)
{
l_tools_checkUserDataPlusErrMsg(state, 1, "You must provide a body");
l_physics_Body* body = (l_physics_Body*)lua_touserdata(state, 1);
lua_pushnumber(state, cpBodyGetAngle(body->body));
return 1;
}
BOOL debugMessageBox(cpBody *body,const char* head)
{
char str[100];
cpVect pos;
pos = cpBodyGetPos(body);
sprintf(str,"Pos:(%f,%f) Angle: %f \0",pos.x,pos.y,cpBodyGetAngle(body));
if (MessageBoxA(NULL,str,head,MB_OKCANCEL) == IDOK) return TRUE;
return FALSE;
}
void RigidBody2D::CopyBodyData(cpBody* body)
{
cpBodySetAngle(m_handle, cpBodyGetAngle(body));
cpBodySetAngularVelocity(m_handle, cpBodyGetAngularVelocity(body));
cpBodySetCenterOfGravity(m_handle, cpBodyGetCenterOfGravity(body));
cpBodySetForce(m_handle, cpBodyGetForce(body));
cpBodySetPosition(m_handle, cpBodyGetPosition(body));
cpBodySetTorque(m_handle, cpBodyGetTorque(body));
cpBodySetVelocity(m_handle, cpBodyGetVelocity(body));
}
void RigidBody2D::CopyBodyData(cpBody* from, cpBody* to)
{
cpBodySetAngle(to, cpBodyGetAngle(from));
cpBodySetAngularVelocity(to, cpBodyGetAngularVelocity(from));
cpBodySetCenterOfGravity(to, cpBodyGetCenterOfGravity(from));
cpBodySetForce(to, cpBodyGetForce(from));
cpBodySetPosition(to, cpBodyGetPosition(from));
cpBodySetTorque(to, cpBodyGetTorque(from));
cpBodySetVelocity(to, cpBodyGetVelocity(from));
}
static JSBool
body_getRotation(JSContext* cx, uintN argc, jsval* vp)
{
JSObject* bodyObj = JS_THIS_OBJECT(cx, vp);
cpBody* body = (cpBody*)JS_GetPrivate(cx, bodyObj);
jsval rVal = DOUBLE_TO_JSVAL(cpBodyGetAngle(body));
JS_SET_RVAL(cx, vp, rVal);
return JS_TRUE;
}
char* NaiveRotationAlgorithm::outputDescription()
{
if (!bestIndividual)
return "";
static char buffer[100];
cpBody *outputBody = bestIndividual->system->partAtPosition(bestIndividual->system->outputMachinePosition)->body;
snprintf(buffer, 100, "Output angle : %.3f", cpBodyGetAngle(outputBody));
return buffer;
}
float PhysicsSprite::getRotation() const
{
#if CC_ENABLE_CHIPMUNK_INTEGRATION
return (_ignoreBodyRotation ? Sprite::getRotation() : -CC_RADIANS_TO_DEGREES(cpBodyGetAngle(_CPBody)));
#elif CC_ENABLE_BOX2D_INTEGRATION
return (_ignoreBodyRotation ? Sprite::getRotation() :
CC_RADIANS_TO_DEGREES(_pB2Body->GetAngle()));
#endif
}
int update_brain(Ship* s)
{
float* inputs = new float[NINPUTS];
float noseang = cpBodyGetAngle(s->body) + s->nose_angle;
cpVect rpos = cpBodyGetPosition(s->target->body) - cpBodyGetPosition(s->body);
inputs[0] = cpvlength(rpos);
inputs[1] = restrictangle(cpvtoangle(rpos)-noseang);
cpVect rvel = cpBodyGetVelocity(s->target->body) - cpBodyGetVelocity(s->body);
inputs[2] = (rpos.x * rvel.x + rpos.y * rvel.y)/inputs[0];
inputs[3] = (rpos.x * rvel.y - rpos.y * rvel.x)/(inputs[0]*inputs[0]) - cpBodyGetAngularVelocity(s->body);
inputs[4] = restrictangle(cpBodyGetAngle(s->target->body) + s->target->nose_angle - noseang);
inputs[5] = cpBodyGetAngularVelocity(s->target->body);
s->brain->update(inputs);
delete inputs;
return 0;
}
int lc_body_index(lua_State *vm){
//userdata, key
const char *key = lua_tostring(vm, 2);
cpBody *body = (lc_GetBody(1, vm))->body;
if (strcmp("pos", key) == 0){
lc_cpVectToTable(cpBodyGetPos(body), vm);
return 1;
}
else if (strcmp("vel", key) == 0){
lc_cpVectToTable(cpBodyGetVel(body), vm);
return 1;
}
else if (strcmp("angle", key) == 0 || strcmp("radangle", key) == 0){
lua_pushnumber(vm, (lua_Number)cpBodyGetAngle(body));
return 1;
}
else if (strcmp("degangle", key) == 0){
lua_pushnumber(vm, (lua_Number)( cpBodyGetAngle(body)*(cpFloat)180.f/(cpFloat)M_PI) );
return 1;
}
lua_getfield(vm, LUA_REGISTRYINDEX, "chipmunk.body:");
lua_getfield(vm, -1, key);
return 1;
}
void Player::handleFire(SDL_Renderer *r, cpSpace *space, utils::Timer & fireTimer, cpFloat fireAngle, bool isclient) {
//If holding the left button
if (!isclient || Lpressed) {
if (fireTimer.exceededReset()) {
if (isclient && mFiredNumber < MAX_FIREANGLES) {
mFiredAngle[ mFiredNumber++ ] = cpBodyGetAngle(body());
}
for( int i=0; i<mMaxAmmo; i++) {
if ( !mAmmo[i].checkExist()) {
mAmmo[i].createBullet(r, space, 700, this, !isclient);
break;
}
}
}
}
}
int applykeys(Ship* s, bool* keylist)
{
if (keylist[0])
cpBodyApplyForceAtLocalPoint(s->body, cpv(0, 1e7), cpv(0.5, 0));
if (keylist[1])
cpBodyApplyForceAtLocalPoint(s->body, cpv(0, -1e6), cpv(0.5, 3));
if (keylist[2])
{
cpBodyApplyForceAtLocalPoint(s->body, cpv(1e6, 0), cpv(0, 0));
cpBodyApplyForceAtLocalPoint(s->body, cpv(-1e6, 0), cpv(1, 3));
}
if (keylist[3])
{
cpBodyApplyForceAtLocalPoint(s->body, cpv(1e6, 0), cpv(0, 3));
cpBodyApplyForceAtLocalPoint(s->body, cpv(-1e6, 0), cpv(1, 0));
}
if (keylist[4])
{
cpBodyApplyForceAtLocalPoint(s->body, cpv(-1e6, 0), cpv(1, 3));
cpBodyApplyForceAtLocalPoint(s->body, cpv(-1e6, 0), cpv(1, 0));
}
if (keylist[5])
{
cpBodyApplyForceAtLocalPoint(s->body, cpv(1e6, 0), cpv(0, 3));
cpBodyApplyForceAtLocalPoint(s->body, cpv(1e6, 0), cpv(0, 0));
}
if (keylist[6] && last_time_updated - s->last_fired > 1)
{
cpVect tip = cpBodyLocalToWorld(s->body, nosev + cpv(0, 0.1));
cpVect base = cpBodyLocalToWorld(s->body, cpv(0.5, 0));
cpVect newvel = cpvnormalize(tip - base) * shell_muzzle_vel + cpBodyGetVelocityAtLocalPoint(s->body, nosev);
Shell* newshell = addshell(tip, newvel, cpBodyGetAngle(s->body));
cpBodyApplyImpulseAtLocalPoint(s->body, cpv(0, -1)*cpBodyGetMass(newshell->body)*shell_muzzle_vel, cpv(0.5, 3));
s->last_fired = last_time_updated;
}
return 0;
}
ETERM *body_get_position(ETERM *fromp, ETERM *argp) {
// get the args
ETERM *space_refp = erl_element(1, argp);
ETERM *idp = erl_element(2, 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 = cpBodyGetAngle(b->body);
ETERM **position_tuple_array = (ETERM **) malloc(sizeof(ETERM*) * 2);
position_tuple_array[0] = erl_mk_float(position.x);
position_tuple_array[1] = erl_mk_float(position.y);
ETERM *position_tuple = erl_mk_tuple(position_tuple_array, 2);
free(position_tuple_array);
ETERM *atom_ok = erl_mk_atom("ok");
ETERM **get_position_array = (ETERM **) malloc(sizeof(ETERM*) * 3);
get_position_array[0] = atom_ok;
get_position_array[1] = position_tuple;
get_position_array[2] = erl_mk_float(angle);
ETERM *get_position_tuple = erl_mk_tuple(get_position_array, 3);
free(get_position_array);
ETERM *reply_tuple = erl_mk_reply(fromp, get_position_tuple);
ETERM *gen_cast_tuple = erl_mk_gen_cast(reply_tuple);
// DEBUGF(("body_get_position(body: %d, x: %f, y: %f, angle: %f) has succeeded",
// body_id, position.x, position.y, angle));
return gen_cast_tuple;
}
void physics_update_all()
{
PhysicsInfo *info;
Entity ent;
entitypool_remove_destroyed(pool, physics_remove);
entitymap_clear(debug_draw_map);
/* simulate */
if (!timing_get_paused())
{
_update_kinematics();
_step();
}
/* synchronize transform <-> physics */
entitypool_foreach(info, pool)
{
ent = info->pool_elem.ent;
/* if transform is dirtier, move to it, else overwrite it */
if (transform_get_dirty_count(ent) != info->last_dirty_count)
{
cpBodySetVel(info->body, cpvzero);
cpBodySetAngVel(info->body, 0.0f);
cpBodySetPos(info->body, cpv_of_vec2(transform_get_position(ent)));
cpBodySetAngle(info->body, transform_get_rotation(ent));
cpSpaceReindexShapesForBody(space, info->body);
}
else if (info->type == PB_DYNAMIC)
{
transform_set_position(ent, vec2_of_cpv(cpBodyGetPos(info->body)));
transform_set_rotation(ent, cpBodyGetAngle(info->body));
}
info->last_dirty_count = transform_get_dirty_count(ent);
}
static JSBool
body_applyDirectionalImpulse(JSContext* cx, uintN argc, jsval* vp)
{
jsdouble amt;
if (!JS_ConvertArguments(cx, argc, JS_ARGV(cx, vp), "d", &amt)) {
/* Throw a JavaScript exception. */
JS_ReportError(cx, "body_applyDirectionalImpulse: couldn't parse out amt");
return JS_FALSE;
}
JSObject* bodyObj = JS_THIS_OBJECT(cx, vp);
cpBody* body = (cpBody*)JS_GetPrivate(cx, bodyObj);
cpFloat angle = cpBodyGetAngle(body);
double xAmt = -sin(angle) * amt;
double yAmt = cos(angle) * amt;
cpVect j = {xAmt, yAmt};
cpBodyApplyImpulse(body, j, cpvzero);
jsval rVal = JSVAL_VOID;
JS_SET_RVAL(cx, vp, rVal);
return JS_TRUE;
}
void
draw_background(SDL_Surface* screen)
{
struct draw_options opts = {
.surface = screen,
.colour = colour(0, 0, 0)
};
sdldraw_rect(opts, 0, 0, SCREEN_WIDTH, SCREEN_HEIGHT);
}
static void
draw_circle_shape(struct draw_options opts, cpShape* shape, cpBody* body)
{
cpVect centre = cpvadd(cpBodyGetPos(body),
cpCircleShapeGetOffset(shape));
cpFloat radius = cpCircleShapeGetRadius(shape);
cpFloat angle = cpBodyGetAngle(body);
cpVect edgePoint = cpvadd(centre,
cpv(radius * cos(angle), radius * sin(angle)));
draw_circle(opts, centre, radius);
draw_line(opts, centre, edgePoint);
}
cpFloat cBody::Angle() const {
return cpBodyGetAngle( mBody );
}
