XCamReturn
V4l2Device::dequeue_buffer(SmartPtr<V4l2Buffer> &buf)
{
struct v4l2_buffer v4l2_buf;
if (!is_activated()) {
XCAM_LOG_DEBUG (
"device(%s) dequeue buffer failed since not activated", XCAM_STR (_name));
return XCAM_RETURN_ERROR_PARAM;
}
xcam_mem_clear (v4l2_buf);
v4l2_buf.type = _capture_buf_type;
v4l2_buf.memory = _memory_type;
if (this->io_control (VIDIOC_DQBUF, &v4l2_buf) < 0) {
XCAM_LOG_ERROR ("device(%s) fail to dequeue buffer.", XCAM_STR (_name));
return XCAM_RETURN_ERROR_IOCTL;
}
XCAM_LOG_DEBUG ("device(%s) dequeue buffer index:%d", XCAM_STR (_name), v4l2_buf.index);
if (v4l2_buf.index > _buf_count) {
XCAM_LOG_ERROR (
"device(%s) dequeue wrong buffer index:%d",
XCAM_STR (_name), v4l2_buf.index);
return XCAM_RETURN_ERROR_ISP;
}
buf = _buf_pool [v4l2_buf.index];
buf->set_timestamp (v4l2_buf.timestamp);
buf->set_timecode (v4l2_buf.timecode);
buf->set_sequence (v4l2_buf.sequence);
//buf.set_length (v4l2_buf.length); // not necessary to set length
return XCAM_RETURN_NO_ERROR;
}
XCamReturn
V4l2Device::request_buffer ()
{
struct v4l2_requestbuffers request_buf;
XCAM_ASSERT (!is_activated());
xcam_mem_clear (request_buf);
request_buf.type = _capture_buf_type;
request_buf.count = _buf_count;
request_buf.memory = _memory_type;
if (io_control (VIDIOC_REQBUFS, &request_buf) < 0) {
XCAM_LOG_INFO ("device(%s) starts failed on VIDIOC_REQBUFS", XCAM_STR (_name));
return XCAM_RETURN_ERROR_IOCTL;
}
if (request_buf.count != _buf_count) {
XCAM_LOG_DEBUG (
"device(%s) request buffer count doesn't match user settings, reset buffer count to %d",
XCAM_STR (_name), request_buf.count);
_buf_count = request_buf.count;
}
return XCAM_RETURN_NO_ERROR;
}
bool
V4l2Device::set_mem_type (enum v4l2_memory type) {
if (is_activated ()) {
XCAM_LOG_WARNING ("device(%s) set mem type failed", XCAM_STR (_name));
return false;
}
_memory_type = type;
return true;
}
bool
V4l2Device::set_buffer_count (uint32_t buf_count)
{
if (is_activated ()) {
XCAM_LOG_WARNING ("device(%s) set buffer count failed", XCAM_STR (_name));
return false;
}
_buf_count = buf_count;
return true;
}
void magnet::update()
{
if(!is_activated())
{
return;
}
auto mag_pos = body_->GetWorldPoint(local_point_);
auto ce = body_->GetContactList();
// std::set< b2Body* > processed;
while(ce)
{
auto contact = ce->contact;
if(contact->IsTouching())
{
auto fixA = contact->GetFixtureA();
auto fixB = contact->GetFixtureB();
// b2Body* obj = nullptr;
b2Fixture* fix = nullptr;
if(fixA == sensor_)
{
// obj = fixB->GetBody();
fix = fixB;
}
else if(fixB == sensor_)
{
// obj = fixA->GetBody();
fix = fixA;
}
// if(obj != nullptr && processed.find(obj) == processed.end())
if(fix)
{
b2MassData md;
fix->GetMassData(&md);
auto metallic_mass = md.mass; // TODO: composition
auto obj_pos = fix->GetBody()->GetWorldPoint(md.center);
auto vec = mag_pos - obj_pos;
auto sqr_dist = vec.LengthSquared();
auto magnitude = strength_ * metallic_mass / sqr_dist;
vec *= magnitude / std::sqrt(sqr_dist);
fix->GetBody()->ApplyForce(vec, obj_pos, true);
body_->ApplyForce(-vec, mag_pos, true);
// processed.insert(obj);
}
}
ce = ce->next;
}
}
XCamReturn
V4l2Device::get_format (struct v4l2_format &format)
{
if (is_activated ()) {
format = _format;
return XCAM_RETURN_NO_ERROR;
}
if (!is_opened ())
return XCAM_RETURN_ERROR_IOCTL;
xcam_mem_clear (format);
format.type = _capture_buf_type;
if (this->io_control (VIDIOC_G_FMT, &format) < 0) {
// FIXME: also log the device name?
XCAM_LOG_ERROR("Fail to get format via ioctl VIDVIO_G_FMT.");
return XCAM_RETURN_ERROR_IOCTL;
}
return XCAM_RETURN_NO_ERROR;
}
XCamReturn
V4l2Device::set_format (struct v4l2_format &format)
{
XCamReturn ret = XCAM_RETURN_NO_ERROR;
XCAM_FAIL_RETURN (ERROR, !is_activated (), XCAM_RETURN_ERROR_PARAM,
"Cannot set format to v4l2 device while it is active.");
XCAM_FAIL_RETURN (ERROR, is_opened (), XCAM_RETURN_ERROR_FILE,
"Cannot set format to v4l2 device while it is closed.");
struct v4l2_format tmp_format = format;
ret = pre_set_format (format);
if (ret != XCAM_RETURN_NO_ERROR) {
XCAM_LOG_WARNING ("device(%s) pre_set_format failed", XCAM_STR (_name));
return ret;
}
if (io_control (VIDIOC_S_FMT, &format) < 0) {
if (errno == EBUSY) {
// TODO log device name
XCAM_LOG_ERROR("Video device is busy, fail to set format.");
} else {
// TODO log format details and errno
XCAM_LOG_ERROR("Fail to set format: %s", strerror(errno));
}
return XCAM_RETURN_ERROR_IOCTL;
}
if (tmp_format.fmt.pix.width != format.fmt.pix.width || tmp_format.fmt.pix.height != format.fmt.pix.height) {
XCAM_LOG_ERROR (
"device(%s) set v4l2 format failed, supported format: width:%d, height:%d",
XCAM_STR (_name),
format.fmt.pix.width,
format.fmt.pix.height);
return XCAM_RETURN_ERROR_PARAM;
}
while (_fps_n && _fps_d) {
struct v4l2_streamparm param;
xcam_mem_clear (param);
param.type = _capture_buf_type;
if (io_control (VIDIOC_G_PARM, ¶m) < 0) {
XCAM_LOG_WARNING ("device(%s) set framerate failed on VIDIOC_G_PARM but continue", XCAM_STR (_name));
break;
}
if (!(param.parm.capture.capability & V4L2_CAP_TIMEPERFRAME))
break;
param.parm.capture.timeperframe.numerator = _fps_d;
param.parm.capture.timeperframe.denominator = _fps_n;
if (io_control (VIDIOC_S_PARM, ¶m) < 0) {
XCAM_LOG_WARNING ("device(%s) set framerate failed on VIDIOC_S_PARM but continue", XCAM_STR (_name));
break;
}
_fps_n = param.parm.capture.timeperframe.denominator;
_fps_d = param.parm.capture.timeperframe.numerator;
XCAM_LOG_INFO ("device(%s) set framerate(%d/%d)", XCAM_STR (_name), _fps_n, _fps_d);
// exit here, otherwise it is an infinite loop
break;
}
ret = post_set_format (format);
if (ret != XCAM_RETURN_NO_ERROR) {
XCAM_LOG_WARNING ("device(%s) post_set_format failed", XCAM_STR (_name));
return ret;
}
_format = format;
XCAM_LOG_INFO (
"device(%s) set format(w:%d, h:%d, pixelformat:%s, bytesperline:%d,image_size:%d)",
XCAM_STR (_name),
format.fmt.pix.width, format.fmt.pix.height,
xcam_fourcc_to_string (format.fmt.pix.pixelformat),
format.fmt.pix.bytesperline,
format.fmt.pix.sizeimage);
return XCAM_RETURN_NO_ERROR;
}
void physics_system::fixtures_construct(const const_entity_handle handle) {
//ensure(!is_constructed_colliders(handle));
if (is_constructed_colliders(handle))
return;
if (handle.has<components::fixtures>()) {
const auto colliders = handle.get<components::fixtures>();
if (colliders.is_activated() && is_constructed_rigid_body(colliders.get_owner_body())) {
const auto& colliders_data = colliders.get_data();
auto& cache = get_colliders_cache(handle);
const auto owner_body_entity = colliders.get_owner_body();
ensure(owner_body_entity.alive());
auto& owner_cache = get_rigid_body_cache(owner_body_entity);
const auto this_cache_id = handle.get_id().pool.indirection_index;
const auto owner_cache_id = owner_body_entity.get_id().pool.indirection_index;
owner_cache.correspondent_colliders_caches.push_back(this_cache_id);
cache.correspondent_rigid_body_cache = owner_cache_id;
for (size_t ci = 0; ci < colliders_data.colliders.size(); ++ci) {
const auto& c = colliders_data.colliders[ci];
b2PolygonShape shape;
b2FixtureDef fixdef;
fixdef.density = c.density;
fixdef.friction = c.friction;
fixdef.isSensor = c.sensor;
fixdef.filter = c.filter;
fixdef.restitution = c.restitution;
fixdef.shape = &shape;
fixdef.userData = handle.get_id();
auto transformed_shape = c.shape;
transformed_shape.offset_vertices(colliders.get_total_offset());
std::vector<b2Fixture*> partitioned_collider;
for (const auto convex : transformed_shape.convex_polys) {
std::vector<b2Vec2> b2verts(convex.vertices.begin(), convex.vertices.end());
for (auto& v : b2verts)
v *= PIXELS_TO_METERSf;
shape.Set(b2verts.data(), b2verts.size());
b2Fixture* const new_fix = owner_cache.body->CreateFixture(&fixdef);
ensure(static_cast<short>(ci) < std::numeric_limits<short>::max());
new_fix->collider_index = static_cast<short>(ci);
partitioned_collider.push_back(new_fix);
}
cache.fixtures_per_collider.push_back(partitioned_collider);
}
}
}
}
