void Triangle::Collide(SecondaryContext &ctx, int idx, int first, int last) const {
Vec3q tnrm(plane.x, plane.y, plane.z);
Vec3q ta(a), tca(ca), tba(ba);
floatq zero(0.0f), one(1.0f), tit0(it0);
int count = last - first + 1;
for(int q = 0; q < count; q++) {
int tq = q + first;
const Vec3q dir = ctx.rayDir[tq];
floatq idet = Inv(dir | tnrm);
Vec3q tvec = ctx.rayOrigin[tq] - ta;
floatq dist = -(tvec | tnrm) * idet;
Vec3q tvec0 = tba ^ tvec;
Vec3q tvec1 = tvec ^ tca;
idet *= tit0;
floatq v = (dir | tvec0) * idet;
floatq u = (dir | tvec1) * idet;
f32x4b test = Min(u, v) >= zero && u + v <= one;
test = test && /*idet > zero &&*/ dist >= zero && dist < ctx.distance[tq];
ctx.distance[tq] = Condition(test, dist, ctx.distance[tq]);
ctx.normals[tq] = Condition(test, tnrm, ctx.normals[tq]);
ctx.triIds[tq] = Condition(i32x4b(test), idx, ctx.triIds[tq]);
ctx.barycentric[tq] = Condition(test, Vec2q(u, v), ctx.barycentric[tq]);
}
}
TEST(TransferTestHelpers, Transfer)
{
uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 8, uavcan::MemPoolBlockSize> pool;
uavcan::TransferBufferManager<128, 1> mgr(pool);
uavcan::TransferBufferAccessor tba(mgr, uavcan::TransferBufferManagerKey(0, uavcan::TransferTypeMessageBroadcast));
uavcan::RxFrame frame(uavcan::Frame(123, uavcan::TransferTypeMessageBroadcast, 1, 0, 0),
uavcan::MonotonicTime(), uavcan::UtcTime(), 0);
frame.setEndOfTransfer(true);
uavcan::MultiFrameIncomingTransfer mfit(tsMono(10), tsUtc(1000), frame, tba);
// Filling the buffer with data
static const std::string TEST_DATA = "Kaneda! What do you see? Kaneda! What do you see? Kaneda! Kaneda!!!";
ASSERT_TRUE(tba.create());
ASSERT_EQ(TEST_DATA.length(), tba.access()->write(0, reinterpret_cast<const uint8_t*>(TEST_DATA.c_str()),
unsigned(TEST_DATA.length())));
// Reading back
const Transfer transfer(mfit, uavcan::DataTypeDescriptor());
ASSERT_EQ(TEST_DATA, transfer.payload);
}
void TransferListenerBase::handleFrame(const RxFrame& frame)
{
if (frame.getSrcNodeID().isUnicast()) // Normal transfer
{
const TransferBufferManagerKey key(frame.getSrcNodeID(), frame.getTransferType());
TransferReceiver* recv = receivers_.access(key);
if (recv == NULL)
{
if (!frame.isFirst())
{
return;
}
TransferReceiver new_recv;
recv = receivers_.insert(key, new_recv);
if (recv == NULL)
{
UAVCAN_TRACE("TransferListener", "Receiver registration failed; frame %s", frame.toString().c_str());
return;
}
}
TransferBufferAccessor tba(bufmgr_, key);
handleReception(*recv, frame, tba);
}
else if (frame.getSrcNodeID().isBroadcast() &&
frame.isFirst() &&
frame.isLast() &&
frame.getDstNodeID().isBroadcast()) // Anonymous transfer
{
handleAnonymousTransferReception(frame);
}
else
{
UAVCAN_TRACE("TransferListenerBase", "Invalid frame: %s", frame.toString().c_str()); // Invalid frame
}
}
bool
BoxList::contains (const BoxArray& ba) const
{
BoxArray tba(*this);
return ba.contains(tba);
}
void Triangle::Collide(ShadowContext &ctx, int idx, int first, int last) const { //40%
Vec3q tnrm(plane.x, plane.y, plane.z);
Vec3q tvec0, tvec1;
floatq tmul; {
Vec3q ta(a), tca(ca), tba(ba);
Vec3q tvec = ctx.rayOrigin - ta;
tvec0 = (tba ^ tvec) * floatq(it0);
tvec1 = (tvec ^ tca) * floatq(it0);
tmul = -(tvec | Vec3q(plane.x, plane.y, plane.z));
}
floatq zero(0.0f), one(1.0f);
while(first <= last) {
int q = first; first += 4;
Vec3q dir[4] = { ctx.rayDir[q + 0], ctx.rayDir[q + 1], ctx.rayDir[q + 2], ctx.rayDir[q + 3] };
f32x4 distance[4] = { ctx.distance[q + 0], ctx.distance[q + 1], ctx.distance[q + 2], ctx.distance[q + 3] };
floatq det[4] = { dir[0] | tnrm, dir[1] | tnrm, dir[2] | tnrm, dir[3] | tnrm };
floatq idet[4] = { Inv(det[0]), Inv(det[1]), Inv(det[2]), Inv(det[3]) };
floatq dist[4] = { idet[0] * tmul, idet[1] * tmul, idet[2] * tmul, idet[3] * tmul };
f32x4b test[4] = {
dist[0] >= zero && dist[0] < distance[0]/* && det[0] >= zero*/,
dist[1] >= zero && dist[1] < distance[1]/* && det[1] >= zero*/,
dist[2] >= zero && dist[2] < distance[2]/* && det[2] >= zero*/,
dist[3] >= zero && dist[3] < distance[3]/* && det[3] >= zero*/ };
floatq tv[4] = {
(dir[0] | tvec0) * idet[0],
(dir[1] | tvec0) * idet[1],
(dir[2] | tvec0) * idet[2],
(dir[3] | tvec0) * idet[3] };
floatq tu[4] = {
(dir[0] | tvec1) * idet[0],
(dir[1] | tvec1) * idet[1],
(dir[2] | tvec1) * idet[2],
(dir[3] | tvec1) * idet[3] };
test[0] = (test[0] && tu[0] >= zero) && (tv[0] >= zero && tu[0] + tv[0] <= one);
test[1] = (test[1] && tu[1] >= zero) && (tv[1] >= zero && tu[1] + tv[1] <= one);
test[2] = (test[2] && tu[2] >= zero) && (tv[2] >= zero && tu[2] + tv[2] <= one);
test[3] = (test[3] && tu[3] >= zero) && (tv[3] >= zero && tu[3] + tv[3] <= one);
ctx.distance[q + 0] = Condition(test[0], dist[0], distance[0]);
ctx.distance[q + 1] = Condition(test[1], dist[1], distance[1]);
ctx.distance[q + 2] = Condition(test[2], dist[2], distance[2]);
ctx.distance[q + 3] = Condition(test[3], dist[3], distance[3]);
}
if(0) while(first <= last) {
int q = first++;
Vec3q dir = ctx.rayDir[q];
floatq distance = ctx.distance[q];
floatq det = dir | tnrm;
floatq idet = Inv(det);
floatq dist = idet * tmul;
floatq v = (dir | tvec0);
floatq u = (dir | tvec1);
f32x4b test = u >= zero && v >= zero && u + v <= det;
test = test /*&& idet > zero*/ && dist >= zero && dist < distance;
ctx.distance[q] = Condition(test, dist, distance);
}
}
void Triangle::Collide(PrimaryContext &ctx, int idx, int first, int last) const { //40%
Vec3q tnrm(plane.x, plane.y, plane.z);
Vec3q tvec0, tvec1;
floatq tmul; {
Vec3q ta(a), tca(ca), tba(ba);
Vec3q tvec = ctx.rayOrigin - ta;
tvec0 = (tba ^ tvec) * floatq(it0);
tvec1 = (tvec ^ tca) * floatq(it0);
tmul = -(tvec | Vec3q(plane.x, plane.y, plane.z));
}
floatq zero(0.0f), one(1.0f);
//TODO: nany i denormalne z koncowek moga wplywac na wydajnosc
while(first <= last) {
int q = first; first += 4;
Vec3q dir[4] = { ctx.rayDir[q + 0], ctx.rayDir[q + 1], ctx.rayDir[q + 2], ctx.rayDir[q + 3] };
i32x4 triIds[4] = { ctx.triIds[q + 0], ctx.triIds[q + 1], ctx.triIds[q + 2], ctx.triIds[q + 3] };
f32x4 distance[4] = { ctx.distance[q + 0], ctx.distance[q + 1], ctx.distance[q + 2], ctx.distance[q + 3] };
floatq idet[4] = { Inv(dir[0] | tnrm), Inv(dir[1] | tnrm), Inv(dir[2] | tnrm), Inv(dir[3] | tnrm) };
f32x4b test[4] = { true, true, true, true };//idet[0] > zero, idet[1] > zero, idet[2] > zero, idet[3] > zero };
floatq dist[4] = { idet[0] * tmul, idet[1] * tmul, idet[2] * tmul, idet[3] * tmul };
floatq v[4] = {
(dir[0] | tvec0) * idet[0], (dir[1] | tvec0) * idet[1],
(dir[2] | tvec0) * idet[2], (dir[3] | tvec0) * idet[3] };
Vec2q barycentric[4] = { ctx.barycentric[q + 0], ctx.barycentric[q + 1], ctx.barycentric[q + 2], ctx.barycentric[q + 3] };
floatq u[4] = {
(dir[0] | tvec1) * idet[0], (dir[1] | tvec1) * idet[1],
(dir[2] | tvec1) * idet[2], (dir[3] | tvec1) * idet[3] };
Vec3q normals[4] = { ctx.normals[q + 0], ctx.normals[q + 1], ctx.normals[q + 2], ctx.normals[q + 3] };
test[0] = ((test[0] && u[0] > zero) && (v[0] > zero && u[0] + v[0] < one))
&& (dist[0] > zero && dist[0] < distance[0]);
test[1] = ((test[1] && u[1] > zero) && (v[1] > zero && u[1] + v[1] < one))
&& (dist[1] > zero && dist[1] < distance[1]);
test[2] = ((test[2] && u[2] > zero) && (v[2] > zero && u[2] + v[2] < one))
&& (dist[2] > zero && dist[2] < distance[2]);
test[3] = ((test[3] && u[3] > zero) && (v[3] > zero && u[3] + v[3] < one))
&& (dist[3] > zero && dist[3] < distance[3]);
ctx.distance[q + 0] = Condition(test[0], dist[0], distance[0]);
ctx.distance[q + 1] = Condition(test[1], dist[1], distance[1]);
ctx.distance[q + 2] = Condition(test[2], dist[2], distance[2]);
ctx.distance[q + 3] = Condition(test[3], dist[3], distance[3]);
ctx.normals[q + 0] = Condition(test[0], tnrm, normals[0]);
ctx.normals[q + 1] = Condition(test[1], tnrm, normals[1]);
ctx.normals[q + 2] = Condition(test[2], tnrm, normals[2]);
ctx.normals[q + 3] = Condition(test[3], tnrm, normals[3]);
ctx.triIds[q + 0] = Condition(i32x4b(test[0]), idx, triIds[0]);
ctx.triIds[q + 1] = Condition(i32x4b(test[1]), idx, triIds[1]);
ctx.triIds[q + 2] = Condition(i32x4b(test[2]), idx, triIds[2]);
ctx.triIds[q + 3] = Condition(i32x4b(test[3]), idx, triIds[3]);
ctx.barycentric[q + 0] = Condition(test[0], Vec2q(u[0], v[0]), barycentric[0]);
ctx.barycentric[q + 1] = Condition(test[1], Vec2q(u[1], v[1]), barycentric[1]);
ctx.barycentric[q + 2] = Condition(test[2], Vec2q(u[2], v[2]), barycentric[2]);
ctx.barycentric[q + 3] = Condition(test[3], Vec2q(u[3], v[3]), barycentric[3]);
}
if(0) while(first <= last) {
int q = first++;
Vec3q dir = ctx.rayDir[q];
floatq distance = ctx.distance[q];
i32x4 triIds = ctx.triIds[q];
floatq idet = Inv(dir | tnrm);
floatq dist = idet * tmul;
Vec2q barycentric = ctx.barycentric[q];
Vec3q normals = ctx.normals[q];
floatq v = (dir | tvec0) * idet;
floatq u = (dir | tvec1) * idet;
f32x4b test = Min(u, v) >= zero && u + v <= one;
test = test && /*idet > zero &&*/ dist >= zero && dist < distance;
ctx.distance[q] = Condition(test, dist, distance);
ctx.normals[q] = Condition(test, tnrm, normals);
ctx.triIds[q] = Condition(i32x4b(test), idx, triIds);
ctx.barycentric[q] = Condition(test, Vec2q(u, v), barycentric);
}
}
