static void post_step_body_replace_shapes(cpSpace *space, cpBody *body, void *data) {
Body_data *pa = cpBodyGetUserData(body);
int length = pa->x_values->len;
cpBodyEachShape(body, (cpBodyShapeIteratorFunc) free_shape, NULL);
cpSpaceReindexShapesForBody(space, body);
//fprintf(stderr, "0Made it this far.\n");
for (int index = 1; index < length; index++) {
cpFloat x = g_array_index(pa->x_values, cpFloat, index);
cpFloat y = g_array_index(pa->y_values, cpFloat, index);
cpShape *seg = cpSegmentShapeNew(body, cpv(x,y), cpv(g_array_index(pa->x_values, cpFloat, index - 1), g_array_index(pa->y_values, cpFloat, index - 1)), CRAYON_RADIUS);
//fprintf(stderr, "1Made it this far.\n");
cpShapeSetFriction(seg, CRAYON_FRICTION);
//fprintf(stderr, "2Made it this far.\n");
//cpShapeSetElasticity(cpShape *shape, cpFloat value)?
cpSpaceAddShape(space, seg);
//fprintf(stderr, "3Made it this far.\n");
}
cpSpaceReindexShapesForBody(space, body);
}
void CDynamics2DMultiBodyObjectModel::MoveTo(const CVector3& c_position,
const CQuaternion& c_orientation) {
/* Set target position and orientation */
cpVect tBodyPos = cpv(c_position.GetX(), c_position.GetY());
CRadians cXAngle, cYAngle, cZAngle;
c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpFloat tBodyOrient = cZAngle.GetValue();
/* For each body: */
for(size_t i = 0; i < m_vecBodies.size(); ++i) {
/* Set body orientation at anchor */
cpBodySetAngle(m_vecBodies[i].Body,
tBodyOrient + m_vecBodies[i].OffsetOrient);
/* Set body position at anchor */
cpBodySetPos(m_vecBodies[i].Body,
cpvadd(tBodyPos,
cpvrotate(m_vecBodies[i].OffsetPos,
m_vecBodies[i].Body->rot)));
/* Update shape index */
cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(),
m_vecBodies[i].Body);
}
/* Update ARGoS entity state */
UpdateEntityStatus();
}
static void _update_kinematics()
{
PhysicsInfo *info;
cpVect pos;
cpFloat ang;
Scalar invdt;
Entity ent;
if (timing_dt <= FLT_EPSILON)
return;
invdt = 1 / timing_dt;
entitypool_foreach(info, pool)
if (info->type == PB_KINEMATIC)
{
ent = info->pool_elem.ent;
/* move to transform */
pos = cpv_of_vec2(transform_get_position(ent));
ang = transform_get_rotation(ent);
cpBodySetPos(info->body, pos);
cpBodySetAngle(info->body, ang);
info->last_dirty_count = transform_get_dirty_count(ent);
/* update linear, angular velocities based on delta */
cpBodySetVel(info->body,
cpvmult(cpvsub(pos, info->last_pos), invdt));
cpBodySetAngVel(info->body, (ang - info->last_ang) * invdt);
cpSpaceReindexShapesForBody(space, info->body);
/* save current state for next computation */
info->last_pos = pos;
info->last_ang = ang;
}
}
void RigidBody2D::SetRotation(float rotation)
{
cpBodySetAngle(m_handle, ToRadians(rotation));
if (m_isStatic)
{
m_world->RegisterPostStep(this, [](Nz::RigidBody2D* body)
{
cpSpaceReindexShapesForBody(body->GetWorld()->GetHandle(), body->GetHandle());
});
}
}
void RigidBody2D::SetPosition(const Vector2f& position)
{
cpBodySetPosition(m_handle, cpv(position.x, position.y));
if (m_isStatic)
{
m_world->RegisterPostStep(this, [](Nz::RigidBody2D* body)
{
cpSpaceReindexShapesForBody(body->GetWorld()->GetHandle(), body->GetHandle());
});
}
}
void CDynamics2DSingleBodyObjectModel::MoveTo(const CVector3& c_position,
const CQuaternion& c_orientation) {
/* Move the body to the desired position */
m_ptBody->p = cpv(c_position.GetX(), c_position.GetY());
CRadians cXAngle, cYAngle, cZAngle;
c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpBodySetAngle(m_ptBody, cZAngle.GetValue());
/* Update shape index */
cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(), m_ptBody);
/* Update ARGoS entity state */
UpdateEntityStatus();
}
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;
}
void RigidBody2D::SetStatic(bool setStaticBody)
{
m_isStatic = setStaticBody;
m_world->RegisterPostStep(this, [](Nz::RigidBody2D* body)
{
if (body->IsStatic())
{
cpBodySetType(body->GetHandle(), CP_BODY_TYPE_STATIC);
cpSpaceReindexShapesForBody(body->GetWorld()->GetHandle(), body->GetHandle());
}
else if (cpBodyGetMass(body->GetHandle()) > 0.f)
cpBodySetType(body->GetHandle(), CP_BODY_TYPE_KINEMATIC);
else
cpBodySetType(body->GetHandle(), CP_BODY_TYPE_DYNAMIC);
});
}
void CDynamics2DSingleBodyObjectModel::Reset() {
/* Nothing to do for a static body */
if(cpBodyIsStatic(m_ptBody)) return;
/* Reset body position */
const CVector3& cPosition = GetEmbodiedEntity().GetOriginAnchor().Position;
m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
/* Reset body orientation */
CRadians cXAngle, cYAngle, cZAngle;
GetEmbodiedEntity().GetOriginAnchor().Orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpBodySetAngle(m_ptBody, cZAngle.GetValue());
/* Zero speed and applied forces */
m_ptBody->v = cpvzero;
m_ptBody->w = 0.0f;
cpBodyResetForces(m_ptBody);
/* Update bounding box */
cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(), m_ptBody);
CalculateBoundingBox();
}
void init_physics()
{
// setup space
space = cpSpaceNew();
// setup tires
int i;
for(i=0; i<1; i++) {
cpFloat x = 0, y = 0, w = 0.5, h = 1.75;
cpFloat mass = 50;
cpFloat moment = cpMomentForBox(mass, w, h);
tire[i] = cpSpaceAddBody(space, cpBodyNew(mass, moment));
cpSpaceAddShape(space, cpBoxShapeNew2(tire[i],
cpBBNew(x-(w/2), y-(h/2), x+(w/2), y+(h/2))));
cpSpaceReindexShapesForBody(space, tire[0]);
cpBodySetAngle(tire[i], -M_PI/6);
//cpBodySetVel(tire[i], cpv(0, 10));
//cpBodyApplyImpulse(tire[i], cpv(0, 1000), cpvzero);
}
}
static JSBool
body_setRotation(JSContext* cx, uintN argc, jsval* vp)
{
jsdouble angleRadiansVal;
JSObject* spaceObj;
if (!JS_ConvertArguments(cx, argc, JS_ARGV(cx, vp), "do", &angleRadiansVal, &spaceObj)) {
/* Throw a JavaScript exception. */
JS_ReportError(cx, "body_setRotation: couldn't parse out angle");
return JS_FALSE;
}
JSObject* bodyObj = JS_THIS_OBJECT(cx, vp);
cpBody* body = (cpBody*)JS_GetPrivate(cx, bodyObj);
cpSpace* space = (cpSpace*)JS_GetPrivate(cx, spaceObj);
cpBodySetAngle(body, angleRadiansVal);
cpSpaceReindexShapesForBody(space, body);
jsval rVal = JSVAL_VOID;
JS_SET_RVAL(cx, vp, rVal);
return JS_TRUE;
}
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 int l_physics_setBodyPosition(lua_State* state)
{
l_tools_checkUserDataPlusErrMsg(state, 1, "You must provide a body");
l_tools_checkUserDataPlusErrMsg(state, 2, "You must provide a space");
l_physics_Body* body = (l_physics_Body*)lua_touserdata(state, 1);
l_physics_PhysicsData* space = (l_physics_PhysicsData*)lua_touserdata(state, 2);
cpVect vec = cpvzero;
vec.x = l_tools_toNumberOrError(state, 3);
vec.y = l_tools_toNumberOrError(state, 4);
cpBodySetPosition(body->body, vec);
/*
* Docs:
* When changing the position you may also want to call cpSpaceReindexShapesForBody()
* to update the collision detection information for the attached shapes if plan to
* make any queries against the space.
*/
cpSpaceReindexShapesForBody(space->physics->space, body->body);
return 0;
}
void Space::reindexShapesForBody(cp::Body *body)
{
cpSpaceReindexShapesForBody(space,body ? body->get() : 0);
}
