mirror of https://github.com/axmolengine/axmol.git
180 lines
6.1 KiB
C
180 lines
6.1 KiB
C
/* Copyright (c) 2007 Scott Lembcke
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in
|
|
* all copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
|
|
#include "chipmunk/chipmunk.h"
|
|
#include "ChipmunkDemo.h"
|
|
|
|
static cpBody *balance_body;
|
|
static cpFloat balance_sin = 0.0;
|
|
//static cpFloat last_v = 0.0;
|
|
|
|
static cpBody *wheel_body;
|
|
static cpConstraint *motor;
|
|
|
|
/*
|
|
TODO:
|
|
- Clamp max angle dynamically based on output torque.
|
|
*/
|
|
|
|
|
|
static inline cpFloat
|
|
bias_coef(cpFloat errorBias, cpFloat dt)
|
|
{
|
|
return 1.0f - cpfpow(errorBias, dt);
|
|
}
|
|
|
|
static void motor_preSolve(cpConstraint *motor, cpSpace *space)
|
|
{
|
|
cpFloat dt = cpSpaceGetCurrentTimeStep(space);
|
|
|
|
cpFloat target_x = ChipmunkDemoMouse.x;
|
|
ChipmunkDebugDrawSegment(cpv(target_x, -1000.0), cpv(target_x, 1000.0), RGBAColor(1.0, 0.0, 0.0, 1.0));
|
|
|
|
cpFloat max_v = 500.0;
|
|
cpFloat target_v = cpfclamp(bias_coef(0.5, dt/1.2)*(target_x - cpBodyGetPosition(balance_body).x)/dt, -max_v, max_v);
|
|
cpFloat error_v = (target_v - cpBodyGetVelocity(balance_body).x);
|
|
cpFloat target_sin = 3.0e-3*bias_coef(0.1, dt)*error_v/dt;
|
|
|
|
cpFloat max_sin = cpfsin(0.6);
|
|
balance_sin = cpfclamp(balance_sin - 6.0e-5*bias_coef(0.2, dt)*error_v/dt, -max_sin, max_sin);
|
|
cpFloat target_a = asin(cpfclamp(-target_sin + balance_sin, -max_sin, max_sin));
|
|
cpFloat angular_diff = asin(cpvcross(cpBodyGetRotation(balance_body), cpvforangle(target_a)));
|
|
cpFloat target_w = bias_coef(0.1, dt/0.4)*(angular_diff)/dt;
|
|
|
|
cpFloat max_rate = 50.0;
|
|
cpFloat rate = cpfclamp(cpBodyGetAngularVelocity(wheel_body) + cpBodyGetAngularVelocity(balance_body) - target_w, -max_rate, max_rate);
|
|
cpSimpleMotorSetRate(motor, cpfclamp(rate, -max_rate, max_rate));
|
|
cpConstraintSetMaxForce(motor, 8.0e4);
|
|
}
|
|
|
|
|
|
static void
|
|
update(cpSpace *space, double dt)
|
|
{
|
|
cpSpaceStep(space, dt);
|
|
}
|
|
|
|
static cpSpace *
|
|
init(void)
|
|
{
|
|
ChipmunkDemoMessageString = "This unicycle is completely driven and balanced by a single cpSimpleMotor.\nMove the mouse to make the unicycle follow it.";
|
|
|
|
cpSpace *space = cpSpaceNew();
|
|
cpSpaceSetIterations(space, 30);
|
|
cpSpaceSetGravity(space, cpv(0, -500));
|
|
|
|
{
|
|
cpShape *shape = NULL;
|
|
cpBody *staticBody = cpSpaceGetStaticBody(space);
|
|
|
|
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-3200,-240), cpv(3200,-240), 0.0f));
|
|
cpShapeSetElasticity(shape, 1.0f);
|
|
cpShapeSetFriction(shape, 1.0f);
|
|
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
|
|
|
|
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(0,-200), cpv(240,-240), 0.0f));
|
|
cpShapeSetElasticity(shape, 1.0f);
|
|
cpShapeSetFriction(shape, 1.0f);
|
|
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
|
|
|
|
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-240,-240), cpv(0,-200), 0.0f));
|
|
cpShapeSetElasticity(shape, 1.0f);
|
|
cpShapeSetFriction(shape, 1.0f);
|
|
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
|
|
}
|
|
|
|
|
|
{
|
|
cpFloat radius = 20.0;
|
|
cpFloat mass = 1.0;
|
|
|
|
cpFloat moment = cpMomentForCircle(mass, 0.0, radius, cpvzero);
|
|
wheel_body = cpSpaceAddBody(space, cpBodyNew(mass, moment));
|
|
cpBodySetPosition(wheel_body, cpv(0.0, -160.0 + radius));
|
|
|
|
cpShape *shape = cpSpaceAddShape(space, cpCircleShapeNew(wheel_body, radius, cpvzero));
|
|
cpShapeSetFriction(shape, 0.7);
|
|
cpShapeSetFilter(shape, cpShapeFilterNew(1, CP_ALL_CATEGORIES, CP_ALL_CATEGORIES));
|
|
}
|
|
|
|
{
|
|
cpFloat cog_offset = 30.0;
|
|
|
|
cpBB bb1 = cpBBNew(-5.0, 0.0 - cog_offset, 5.0, cog_offset*1.2 - cog_offset);
|
|
cpBB bb2 = cpBBNew(-25.0, bb1.t, 25.0, bb1.t + 10.0);
|
|
|
|
cpFloat mass = 3.0;
|
|
cpFloat moment = cpMomentForBox2(mass, bb1) + cpMomentForBox2(mass, bb2);
|
|
|
|
balance_body = cpSpaceAddBody(space, cpBodyNew(mass, moment));
|
|
cpBodySetPosition(balance_body, cpv(0.0, cpBodyGetPosition(wheel_body).y + cog_offset));
|
|
|
|
cpShape *shape = NULL;
|
|
|
|
shape = cpSpaceAddShape(space, cpBoxShapeNew2(balance_body, bb1, 0.0));
|
|
cpShapeSetFriction(shape, 1.0);
|
|
cpShapeSetFilter(shape, cpShapeFilterNew(1, CP_ALL_CATEGORIES, CP_ALL_CATEGORIES));
|
|
|
|
shape = cpSpaceAddShape(space, cpBoxShapeNew2(balance_body, bb2, 0.0));
|
|
cpShapeSetFriction(shape, 1.0);
|
|
cpShapeSetFilter(shape, cpShapeFilterNew(1, CP_ALL_CATEGORIES, CP_ALL_CATEGORIES));
|
|
}
|
|
|
|
cpVect anchorA = cpBodyWorldToLocal(balance_body, cpBodyGetPosition(wheel_body));
|
|
cpVect groove_a = cpvadd(anchorA, cpv(0.0, 30.0));
|
|
cpVect groove_b = cpvadd(anchorA, cpv(0.0, -10.0));
|
|
cpSpaceAddConstraint(space, cpGrooveJointNew(balance_body, wheel_body, groove_a, groove_b, cpvzero));
|
|
cpSpaceAddConstraint(space, cpDampedSpringNew(balance_body, wheel_body, anchorA, cpvzero, 0.0, 6.0e2, 30.0));
|
|
|
|
motor = cpSpaceAddConstraint(space, cpSimpleMotorNew(wheel_body, balance_body, 0.0));
|
|
cpConstraintSetPreSolveFunc(motor, motor_preSolve);
|
|
|
|
{
|
|
cpFloat width = 100.0;
|
|
cpFloat height = 20.0;
|
|
cpFloat mass = 3.0;
|
|
|
|
cpBody *boxBody = cpSpaceAddBody(space, cpBodyNew(mass, cpMomentForBox(mass, width, height)));
|
|
cpBodySetPosition(boxBody, cpv(200, -100));
|
|
|
|
cpShape *shape = cpSpaceAddShape(space, cpBoxShapeNew(boxBody, width, height, 0.0));
|
|
cpShapeSetFriction(shape, 0.7);
|
|
}
|
|
|
|
return space;
|
|
}
|
|
|
|
static void
|
|
destroy(cpSpace *space)
|
|
{
|
|
ChipmunkDemoFreeSpaceChildren(space);
|
|
cpSpaceFree(space);
|
|
}
|
|
|
|
ChipmunkDemo Unicycle = {
|
|
"Unicycle",
|
|
1.0/60.0,
|
|
init,
|
|
update,
|
|
ChipmunkDemoDefaultDrawImpl,
|
|
destroy,
|
|
};
|