void QuadTree::Initialzie(ID3D11Device* Device, UINT m, UINT n, Vertex* vertex,int layer,int ext,XMFLOAT3 Center)
{
vertexs = new Vertex[m*n];
for (int i = 0; i < m*n; i++)
{
vertexs[i] = vertex[i];
}
nrVertexes = m*n;
Children = new QuadTree[4];
//VERTEXES
// ___________________ ^
// | | | |
// | A | B | |
// | | | |
// |---------|---------| m
// | | | |
// | C | D | |
// |_________|_________| v
// <---------n--------->
if (layer==1)
{
Vertex* Vtemp1 = new Vertex[m*n / 4 + m + n];
Vertex* Vtemp2 = new Vertex[m*n / 4 + m + n];
Vertex* Vtemp3 = new Vertex[m*n / 4 + m + n];
Vertex* Vtemp4 = new Vertex[m*n / 4 + m + n];
// A
int added = 0;
for (int i = 0; i <= m / 2; ++i)
{
for (int j = 0; j <= n / 2; ++j)
{
Vtemp1[added++] = vertex[i * m + j];
}
}
// B
added = 0;
for (int i = 0; i <= m / 2; ++i)
{
for (int j = n / 2 - 1; j < n; ++j)
{
Vtemp2[added++] = vertex[i * m + j];
}
}
// C
added = 0;
for (int i = m / 2 - 1; i < m; ++i)
{
for (int j = 0; j <= n / 2; ++j)
{
Vtemp3[added++] = vertex[i * m + j];
}
}
// D
added = 0;
for (int i = m / 2 - 1; i < m; ++i)
{
for (int j = n / 2 - 1; j < n; ++j)
{
Vtemp4[added++] = vertex[i * m + j];
}
}
Children[0].Initialzie(Device, m / 2+1, n / 2+1, Vtemp1, layer - 1, ext / 2, XMFLOAT3(Center.x - ext / 2, 0.0, Center.z + ext / 2));
Children[1].Initialzie(Device, m / 2+1, n / 2+1, Vtemp2, layer - 1, ext / 2, XMFLOAT3(Center.x + ext / 2, 0.0, Center.z + ext / 2));
Children[2].Initialzie(Device, m / 2+1, n / 2+1, Vtemp3, layer - 1, ext / 2, XMFLOAT3(Center.x - ext / 2, 0.0, Center.z - ext / 2));
Children[3].Initialzie(Device, m / 2+1, n / 2+1, Vtemp4, layer - 1, ext / 2, XMFLOAT3(Center.x + ext / 2, 0.0, Center.z - ext / 2));
delete[] Vtemp1;
delete[] Vtemp2;
delete[] Vtemp3;
delete[] Vtemp4;
}
else if (layer == 0)
{
int faceCount = (m - 1)*(n - 1) * 2;
nrIndices = faceCount * 3;
int k = 0;
indices = new UINT[nrIndices];
for (int i = 0; i < m - 1; i++)
{
for (int j = 0; j < n - 1; j++)
{
indices[k] = i*(n)+j;
indices[k + 1] = i*(n)+j + 1;
indices[k + 2] = (i + 1)*(n)+j;
indices[k + 3] = (i + 1)*(n)+j;
indices[k + 4] = i*(n)+j + 1;
indices[k + 5] = (i + 1)*(n)+j + 1;
k += 6;
}
}
leaf = true;
// indexBuffer
D3D11_BUFFER_DESC IndexBufferDesc;
IndexBufferDesc.Usage = D3D11_USAGE_DEFAULT;
IndexBufferDesc.ByteWidth = nrIndices*sizeof(UINT);
IndexBufferDesc.BindFlags = D3D11_BIND_INDEX_BUFFER;
IndexBufferDesc.CPUAccessFlags = 0;
IndexBufferDesc.MiscFlags = 0;
IndexBufferDesc.StructureByteStride = 0;
D3D11_SUBRESOURCE_DATA Indexdata;
Indexdata.pSysMem = indices;
Device->CreateBuffer(&IndexBufferDesc, &Indexdata, &IndexB);
D3D11_BUFFER_DESC vbdesc;
ZeroMemory(&vbdesc, sizeof(vbdesc));
vbdesc.Usage = D3D11_USAGE_DEFAULT;
vbdesc.ByteWidth = sizeof(Vertex)*m*n;
vbdesc.BindFlags = D3D11_BIND_VERTEX_BUFFER;
vbdesc.CPUAccessFlags = 0;
vbdesc.MiscFlags = 0;
vbdesc.StructureByteStride = 0;
D3D11_SUBRESOURCE_DATA Data;
ZeroMemory(&Data, sizeof(Data));
Data.pSysMem = vertexs;
Device->CreateBuffer(&vbdesc, &Data, &VertexB);
}
else
{
Vertex* Vtemp1 = new Vertex[m*n / 4];
Vertex* Vtemp2 = new Vertex[m*n / 4];
Vertex* Vtemp3 = new Vertex[m*n / 4];
Vertex* Vtemp4 = new Vertex[m*n / 4];
// A
int added = 0;
for (int i = 0; i < m / 2; ++i)
{
for (int j = 0; j < n / 2; ++j)
{
Vtemp1[added++] = vertex[i * m + j];
}
}
// B
added = 0;
for (int i = 0; i < m / 2; ++i)
{
for (int j = n / 2 ; j < n; ++j)
{
Vtemp2[added++] = vertex[i * m + j];
}
}
// C
added = 0;
for (int i = m / 2; i < m; ++i)
{
for (int j = 0; j < n / 2; ++j)
{
Vtemp3[added++] = vertex[i * m + j];
}
}
// D
added = 0;
for (int i = m / 2; i < m; ++i)
{
for (int j = n / 2 ; j < n; ++j)
{
Vtemp4[added++] = vertex[i * m + j];
}
}
Children[0].Initialzie(Device, m / 2, n / 2, Vtemp1, layer - 1, ext / 2, XMFLOAT3(Center.x - ext / 2, 0.0, Center.z + ext / 2));
Children[1].Initialzie(Device, m / 2, n / 2, Vtemp2, layer - 1, ext / 2, XMFLOAT3(Center.x + ext / 2, 0.0, Center.z + ext / 2));
Children[2].Initialzie(Device, m / 2, n / 2, Vtemp3, layer - 1, ext / 2, XMFLOAT3(Center.x - ext / 2, 0.0, Center.z - ext / 2));
Children[3].Initialzie(Device, m / 2, n / 2, Vtemp4, layer - 1, ext / 2, XMFLOAT3(Center.x + ext / 2, 0.0, Center.z - ext / 2));
delete[] Vtemp1;
delete[] Vtemp2;
delete[] Vtemp3;
delete[] Vtemp4;
}
BoundingBox testBox(Center, XMFLOAT3(ext, 100, ext));
box = testBox;
}
int main(int argc, char **argv)
{
WMScreen *scr;
WMPixmap *pixmap;
/* Initialize the application */
WMInitializeApplication("Test@eqweq_ewq$eqw", &argc, argv);
testUD();
/*
* Open connection to the X display.
*/
dpy = XOpenDisplay("");
if (!dpy) {
puts("could not open display");
exit(1);
}
/* This is used to disable buffering of X protocol requests.
* Do NOT use it unless when debugging. It will cause a major
* slowdown in your application
*/
#if 0
XSynchronize(dpy, True);
#endif
/*
* Create screen descriptor.
*/
scr = WMCreateScreen(dpy, DefaultScreen(dpy));
/*
* Loads the logo of the application.
*/
pixmap = WMCreatePixmapFromFile(scr, "logo.xpm");
/*
* Makes the logo be used in standard dialog panels.
*/
if (pixmap) {
WMSetApplicationIconPixmap(scr, pixmap);
WMReleasePixmap(pixmap);
}
/*
* Do some test stuff.
*
* Put the testSomething() function you want to test here.
*/
testText(scr);
testFontPanel(scr);
testColorPanel(scr);
testTextField(scr);
#if 0
testBox(scr);
testButton(scr);
testColorPanel(scr);
testColorWell(scr);
testDragAndDrop(scr);
testFrame(scr);
testGradientButtons(scr);
testList(scr);
testOpenFilePanel(scr);
testProgressIndicator(scr);
testPullDown(scr);
testScrollView(scr);
testSlider(scr);
testSplitView(scr);
testTabView(scr);
testTextField(scr);
#endif
/*
* The main event loop.
*
*/
WMScreenMainLoop(scr);
return 0;
}
/*************************************************************************************************
*** performs one step (includes learning).
*** Calculates motor commands from sensor inputs.
*** @param sensors sensors inputs scaled to [-1,1]
*** @param sensornumber length of the sensor array
*** @param motors motors outputs. MUST have enough space for motor values!
*** @param motornumber length of the provided motor array
*************************************************************************************************/
void FSMController::step(const sensor* sensors, int sensornumber,
motor* motors, int motornumber){
assert(number_sensors == sensornumber);
assert(number_motors == motornumber);
/*****************************************************************************************/
// motors 0-4
// motor 0 = left front motor
// motor 1 = right front motor
// motor 2 = left hind motor
// motor 3 = right hind motor
// sensors 0-3: wheel velocity of the corresponding wheel
// sensor 0 = wheel velocity left front
// sensor 1 = wheel velocity right front
// sensor 2 = wheel velocity left hind
// sensor 3 = wheel velocity right hind
// sensors 4-9: IR Sensors
// sensor 4 = front middle right IR
// sensor 5 = front middle left IR
// sensor 6 = front right long range IR
// sensor 7 = front left long range IR
// sensor 8 = front right short range IR
// sensor 9 = front left short range IR
// sensors 10-33: distance to obstacles local coordinates (x,y,z)
// sensor 10 = x direction to the first object (goal detection sensor)
// sensor 11 = y direction to the first object (goal detection sensor)
// sensor 12 = z direction to the first object (goal detection sensor)
// 10-12 : Landmark 1 (0)
// 13-15 : Landmark 2 (1)
// 16-18 : Landmark 3 (2)
// 19-21 : Landmark 4 (3)
// 22-24 : Goal (4)
// 25-27 : Box 1 (5)
// 28-30 : Box 2 (6)
// 31-33 : Box 3 (7)
/*****************************************************************************************/
// calculate relative distances and angles from sensor value, normalized to 0..1 for distance and -1..1 for angle
calculateDistanceToGoals(sensors);
calculateAnglePositionFromSensors(sensors);
// smooth ir sensors
for (int i=0;i<6;i++) irSmooth[i] += (sensors[4+i]-irSmooth[i])/smoothingFactor;
// for (int i=0;i<8;i++) parameter.at(2*i) = distances[i];
// for (int i=0;i<8;i++) parameter.at((2*i)+1) = angles[i];
/*** // DSW forward for a bit then backforward for grasping test
counter++;
if (counter < 420) {
vehicle->addGrippables(grippables);
speed = 1.0;
} else if (counter < 700) {
speed = 0.0;
} else if (counter < 1200) {
vehicle->removeAllGrippables();
} else if (counter < 1300){
speed = -1.0;
} else {
speed = 1.0;
}
for (int i = 0; i < number_motors; i++){
motors[i]=speed;
}
*/
/********************************************************************************************
*** FSM
********************************************************************************************/
parameter.at(0) = irSmooth[3];
parameter.at(1) = irSmooth[2];
parameter.at(2) = irSmooth[5];
parameter.at(3) = irSmooth[4];
counter++;
if (state==0) {
vehicle->addGrippables(grippables);
done = false;
boxGripped = false;
testBoxCounter = 0;
dropBoxCounter = 0;
crossGapCounter = 0;
setTarget();
state++;
smoothingFactor = 1;
} else if (state==1){
if(!done) done = goToRandomBox(distances[currentBox],angles[currentBox],motors);
else {
state++;
done = false;
}
} else if (state==2){
if (!done) done = testBox(distances[currentBox],motors,testBoxCounter, boxGripped);
else {
done = false;
if (boxGripped) {
state++;
counter = 0;
}else state=0;
}
} else if (state==3){
if (!done) done = moveToEdge(irSmooth[3],irSmooth[2],motors);
else {
done = false;
state++;
}
} else if (state==4){
if (!done) done = orientAtEdge(irSmooth[3],irSmooth[2],irSmooth[5],irSmooth[4],motors);
else {
done = false;
state++;
dropBoxCounter = counter;
}
} else if (state==5){
if (!done) done = dropBox(vehicle, dropBoxCounter, boxGripped);
else {
done = false;
state++;
}
} else if (state==6){
if (!done) done = crossGap(motors, crossGapCounter);
else {
done = false;
state++;
}
}
};
// new define
void
LevelFluxRegisterEdge::define(
const DisjointBoxLayout& a_dbl,
const DisjointBoxLayout& a_dblCoarse,
const ProblemDomain& a_dProblem,
int a_nRefine,
int a_nComp)
{
m_isDefined = true;
CH_assert(a_nRefine > 0);
CH_assert(a_nComp > 0);
CH_assert(!a_dProblem.isEmpty());
m_nComp = a_nComp;
m_nRefine = a_nRefine;
m_domainCoarse = coarsen(a_dProblem, a_nRefine);
CH_assert (a_dblCoarse.checkPeriodic(m_domainCoarse));
// allocate copiers
m_crseCopiers.resize(SpaceDim*2);
SideIterator side;
// create a Vector<Box> of the fine boxes which also includes periodic images,
// since we don't really care about the processor layouts, etc
Vector<Box> periodicFineBoxes;
CFStencil::buildPeriodicVector(periodicFineBoxes, a_dProblem, a_dbl);
// now coarsen these boxes...
for (int i=0; i<periodicFineBoxes.size(); i++)
{
periodicFineBoxes[i].coarsen(m_nRefine);
}
for (int idir=0 ; idir<SpaceDim; ++idir)
{
for (side.begin(); side.ok(); ++side)
{
// step one, build fineBoxes, flux register boxes
// indexed by the fine level but in the coarse index
// space
DisjointBoxLayout fineBoxes,tmp;
// first create coarsened dbl, then compute flux register boxes
// adjacent to coarsened fine boxes
coarsen(tmp, a_dbl, m_nRefine);
if (side() == Side::Lo)
{
adjCellLo(fineBoxes, tmp, idir,1);
}
else
{
adjCellHi(fineBoxes, tmp, idir,1);
}
// now define the FluxBoxes of fabFine on this DisjointBoxLayout
m_fabFine[index(idir, side())].define(fineBoxes, a_nComp);
LayoutData<Vector<Vector<IntVectSet> > >& ivsetsVect
= m_refluxLocations[index(idir, side())];
ivsetsVect.define(a_dblCoarse);
LayoutData<Vector<DataIndex> >& mapsV =
m_coarToCoarMap[index(idir, side())];
mapsV.define(a_dblCoarse);
DisjointBoxLayout coarseBoxes = a_dblCoarse;
DataIterator dit = a_dblCoarse.dataIterator();
for (dit.begin(); dit.ok(); ++dit)
{
unsigned int thisproc = a_dblCoarse.procID(dit());
if (thisproc == procID())
{
ivsetsVect[DataIndex(dit())].resize(SpaceDim);
}
const Box& coarseBox = a_dblCoarse[dit()];
int count = 0;
for (int i=0; i<periodicFineBoxes.size(); i++)
{
Box regBox;
if (side() == Side::Lo)
{
regBox = adjCellLo(periodicFineBoxes[i], idir, 1);
}
else
{
regBox = adjCellHi(periodicFineBoxes[i], idir, 1);
}
// do this little dance in order to ensure that
// we catch corner cells which might be in different
// boxes.
Box testBox(regBox);
testBox.grow(1);
testBox.grow(idir,-1);
if (testBox.intersectsNotEmpty(coarseBox))
{
testBox &= coarseBox;
++count;
unsigned int proc = a_dblCoarse.procID(dit());
const DataIndex index = DataIndex(dit());
if (proc == procID())
{
mapsV[DataIndex(dit())].push_back(index);
// loop over face directions here
for (int faceDir=0; faceDir<SpaceDim; faceDir++)
{
// do nothing in normal direction
if (faceDir != idir)
{
// this should give us the face indices for the
// faceDir-centered faces adjacent to the coarse-fine
// interface which are contained in the current
// coarse box
Box intersectBox(regBox);
Box coarseEdgeBox(coarseBox);
coarseEdgeBox.surroundingNodes(faceDir);
intersectBox.surroundingNodes(faceDir);
intersectBox &= coarseEdgeBox;
intersectBox.shiftHalf(faceDir,1);
IntVectSet localIV(intersectBox);
ivsetsVect[DataIndex(dit())][faceDir].push_back(localIV);
}
}
}
}
} // end loop over boxes on coarse level
}
m_regCoarse.define(coarseBoxes, a_nComp, IntVect::Unit);
// last thing to do is to define copiers
m_crseCopiers[index(idir, side())].define(fineBoxes, coarseBoxes,
IntVect::Unit);
}
}
}
void occlusionCulling(std::list<std::shared_ptr<DRBData>> &list, bool drawDebugLines)
{
SCOPE_profile_cpu_function("Camera system");
auto depthMap = GetRenderThread()->getDepthMapManager().getReadableMap();
//for (auto &e : cameraList->meshs)
//{
// readableDepthMap->testBox()
//}
if (depthMap.isValid() == false)
{
return;
}
auto j = list.begin();
while (j != std::end(list))
{
auto &d = *j;
auto mesh = std::static_pointer_cast<DRBMeshData>(d);
if (mesh->hadRenderMode(AGE_OCCLUDER))
{
++j;
continue;
}
auto BB = mesh->getAABB();
glm::vec2 minPoint = glm::vec2(1);
glm::vec2 maxPoint = glm::vec2(-1);
float minZ = std::numeric_limits<float>::max();
for (std::size_t i = 0; i < 8; ++i)
{
auto point = depthMap->getMV() * d->getTransformation() * glm::vec4(BB.getCornerPoint(i), 1.0f);
point /= point.w;
if (point.x < -1)
{
point.x = -1;
}
if (point.y < -1)
{
point.y = -1;
}
if (point.x > 1)
{
point.x = 1;
}
if (point.y > 1)
{
point.y = 1;
}
minPoint.x = std::min(minPoint.x, point.x);
minPoint.y = std::min(minPoint.y, point.y);
maxPoint.x = std::max(maxPoint.x, point.x);
maxPoint.y = std::max(maxPoint.y, point.y);
point.z = (point.z + 1.0f) * 0.5f;
minZ = std::min(minZ, point.z);
}
glm::uvec2 screenMin(((minPoint + glm::vec2(1)) / glm::vec2(2)) * glm::vec2(depthMap->getMipmapWidth(), depthMap->getMipmapHeight()));
glm::uvec2 screenMax(((maxPoint + glm::vec2(1)) / glm::vec2(2)) * glm::vec2(depthMap->getMipmapWidth(), depthMap->getMipmapHeight()));
if (minZ < 0)
{
minZ = 0;
}
if (depthMap->testBox((uint32_t)(minZ * (1 << 24)), screenMin, screenMax) == false)
{
list.erase(j++);
continue;
}
if (drawDebugLines)
{
GetRenderThread()->getQueue()->emplaceTask<AGE::Commands::ToRender::Draw2DQuad>(glm::vec2(minPoint.x, minPoint.y), glm::vec2(minPoint.x, maxPoint.y), glm::vec2(maxPoint.x, maxPoint.y), glm::vec2(maxPoint.x, minPoint.y), glm::vec3(0, 1, 0));
}
++j;
}
}
void Game::onInit()
{
m_factory.setResources(this);
m_factory.setWorld(&m_world);
m_factory.setGame(this);
m_factory.init();
m_background = loadTexture("textures/background.png");
glGenFramebuffers(1, &m_lightFB);
glGenTextures(1, &m_lightTexture);
// Setup light texture
glBindFramebuffer(GL_FRAMEBUFFER, m_lightFB);
glBindTexture(GL_TEXTURE_2D, m_lightTexture);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, 800, 600, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, m_lightTexture, 0);
glBindTexture(GL_TEXTURE_2D, 0);
glBindFramebuffer(GL_FRAMEBUFFER, 0);
const float width = 800;
const float height = 2400;
// Light
{
std::unique_ptr<Entities::Light> light(m_factory.createLight());
light->setActive(true);
//light->setPos(b2Vec2(width*0.5f, 150));
light->init();
light->body()->SetTransform(worldToPhysics(b2Vec2(70, 200)), 0);
m_defaultLight = light.get();
m_renderables.push_back(std::unique_ptr<Renderable>(light.release()));
}
// Left
{
std::unique_ptr<Entities::Box> testBox(m_factory.createBricks(b2Vec2(60, height), true));
testBox->body()->SetTransform(worldToPhysics(b2Vec2(0, height*0.5f)), 0);
m_ground = testBox.get()->body().body();
m_renderables.push_back(std::unique_ptr<Renderable>(testBox.release()));
}
// Right
{
std::unique_ptr<Entities::Box> testBox(m_factory.createBricks(b2Vec2(60, height), true));
testBox->body()->SetTransform(worldToPhysics(b2Vec2(width, height*0.5f)), 0);
m_renderables.push_back(std::unique_ptr<Renderable>(testBox.release()));
}
// Bottom
{
std::unique_ptr<Entities::Box> testBox(m_factory.createBricks(b2Vec2(width, 60), true));
testBox->body()->SetTransform(worldToPhysics(b2Vec2(width*0.5f, height)), 0);
m_renderables.push_back(std::unique_ptr<Renderable>(testBox.release()));
}
// Light joint
{
b2DistanceJointDef jointDef;
jointDef.Initialize(m_ground, m_defaultLight->body().body(),
b2Vec2(0, m_defaultLight->body()->GetPosition().y),
m_defaultLight->body()->GetPosition());
jointDef.collideConnected = true;
m_world.CreateJoint(&jointDef);
}
// Supports
for(int i = 0; i < 20; i++)
{
std::unique_ptr<Entities::Box> testBox(m_factory.createHalfBricks(b2Vec2(200, 20)));
testBox->body()->SetTransform(worldToPhysics(b2Vec2(-80, -40 - 40*i)), 0);
b2MouseJointDef jointDef;
jointDef.maxForce = 100;
jointDef.collideConnected = true;
jointDef.bodyA = m_ground;
jointDef.bodyB = testBox->body().body();
jointDef.target = testBox->body()->GetPosition();
testBox->setDragJoint(m_world.CreateJoint(&jointDef));
testBox->body()->SetFixedRotation(true);
m_renderables.push_back(std::unique_ptr<Renderable>(testBox.release()));
}
// Boxes
for(int i = 0; i < 500; i++)
{
int row = i / 3;
int col = i % 3;
int xoff = col - 1;
float xmod = (row & 1) ? 0 : 30;
b2Vec2 pos = worldToPhysics(b2Vec2(400 + xmod + xoff*55, row*-60));
if(!(i % 36))
{
std::unique_ptr<Entities::Light> light(m_factory.createLight());
light->setScale(0.3f);
light->init();
light->body()->SetTransform(pos, 0);
m_renderables.push_back(std::unique_ptr<Renderable>(light.release()));
}
else if((rand() / float(RAND_MAX)) > 0.99f)
{
std::unique_ptr<Entities::Box> testBox(m_factory.createChest());
testBox->body()->SetTransform(pos, 0);
addRenderable(testBox.release());
}
else
{
std::unique_ptr<Entities::Box> testBox(m_factory.createDirt());
testBox->body()->SetTransform(pos, 0);
addRenderable(testBox.release());
}
}
updateCamera();
for(int i = 0; i < 600; i++)
m_world.Step(1/60.f, 6, 2);
}
