void dng_image::Put (const dng_pixel_buffer &buffer)
{
// Move the overlapping pixels.
dng_rect overlap = buffer.fArea & fBounds;
if (overlap.NotEmpty ())
{
dng_pixel_buffer temp (buffer);
temp.fArea = overlap;
temp.fData = (void *) buffer.ConstPixel (overlap.t,
overlap.l,
buffer.fPlane);
// Move the overlapping planes.
if (temp.fPlane < Planes ())
{
temp.fPlanes = Min_uint32 (temp.fPlanes,
Planes () - temp.fPlane);
DoPut (temp);
}
}
}
dng_image * dng_simple_image::Clone () const
{
AutoPtr<dng_simple_image> result (new dng_simple_image (Bounds (),
Planes (),
PixelType (),
fAllocator));
result->fBuffer.CopyArea (fBuffer,
Bounds (),
0,
Planes ());
return result.Release ();
}
std::vector<vgl_plane_3d<double> > GetAllPlanes(const vgl_box_3d<double> &B)
{
std::vector<vgl_plane_3d<double> > Planes(6);
Planes[0] = GetFrontPlane(B);
Planes[1] = GetBackPlane(B);
Planes[2] = GetLeftPlane(B);
Planes[3] = GetRightPlane(B);
Planes[4] = GetTopPlane(B);
Planes[5] = GetBottomPlane(B);
return Planes;
}
bool CEGASprit::loadData(const std::string& filename, bool compresseddata)
{
byte *RawData;
SDL_Surface *sfc;
Uint8* pixel;
Uint32 percent = 0;
FILE* latchfile = OpenGameFile(filename.c_str(),"rb");
if(!latchfile)
return false;
gResourceLoader.setPermilage(10);
RawData = new byte[m_planesize * 5];
// get the data out of the file into the memory, decompressing it if necessary.
if (compresseddata)
{
if (lz_decompress(latchfile, RawData))
return 1;
}
else
{
for(int i=0 ; i<(m_planesize*5) ; i++)
RawData[i] = fgetc(latchfile);
}
fclose(latchfile);
gResourceLoader.setPermilage(50);
// TODO: Try to blit the Font map here!
// these are the offsets of the different video planes as
// relative to each other--that is if a pixel in plane1
// is at N, the byte for that same pixel in plane3 will be
// at (N + plane3).
unsigned long plane1, plane2, plane3, plane4, plane5;
plane1 = 0;
plane2 = (m_planesize * 1);
plane3 = (m_planesize * 2);
plane4 = (m_planesize * 3);
plane5 = (m_planesize * 4);
CPlanes Planes(RawData + m_spriteloc);
Planes.setOffsets(plane1, plane2, plane3, plane4, plane5);
// load the image data
gGraphics.createEmptySprites(4,MAX_SPRITES+1);
for(int i=0 ; i<m_numsprites ; i++)
{
GsSprite &Sprite = gGraphics.getSprite(0,i);
Sprite.setSize( EGASpriteModell[i].width, EGASpriteModell[i].height );
Sprite.setBoundingBoxCoordinates( (EGASpriteModell[i].hitbox_l << STC),
(EGASpriteModell[i].hitbox_u << STC),
(EGASpriteModell[i].hitbox_r << STC),
(EGASpriteModell[i].hitbox_b << STC) );
Sprite.createSurface( gVideoDriver.mpVideoEngine->getBlitSurface()->flags,
gGraphics.Palette.m_Palette );
percent = (i*50)/m_numsprites;
gResourceLoader.setPermilage(50+percent);
}
gResourceLoader.setPermilage(100);
for(int p=0 ; p<4 ; p++)
{
for(int s=0 ; s<m_numsprites ; s++)
{
sfc = gGraphics.getSprite(0,s).getSDLSurface();
if(SDL_MUSTLOCK(sfc)) SDL_LockSurface(sfc);
pixel = (Uint8*) sfc->pixels;
Planes.readPlane(p, pixel, sfc->w, sfc->h);
if(SDL_MUSTLOCK(sfc)) SDL_UnlockSurface(sfc);
percent = (s*100)/m_numsprites;
gResourceLoader.setPermilage(100+percent);
}
}
gResourceLoader.setPermilage(200);
// now load the 5th plane, which contains the sprite masks.
// note that we invert the mask because our graphics functions
// use white on black masks whereas keen uses black on white.
for(int s=0 ; s<m_numsprites ; s++)
{
GsSprite &Sprite = gGraphics.getSprite(0,s);
SDL_Surface *pixsfc = Sprite.getSDLSurface();
SDL_Surface *masksfc = Sprite.getSDLMaskSurface();
if(SDL_MUSTLOCK(pixsfc)) SDL_LockSurface(pixsfc);
if(SDL_MUSTLOCK(masksfc)) SDL_LockSurface(masksfc);
pixel = (Uint8*) masksfc->pixels;
for(int y=0 ; y<masksfc->h ; y++)
{
for(int x=0 ; x<masksfc->w ; x++)
{
if(Planes.getbit(4))
pixel[y*masksfc->w + x] = ((Uint8*)pixsfc->pixels)[y*pixsfc->w + x];
else
pixel[y*masksfc->w + x] = 15;
}
}
if(SDL_MUSTLOCK(masksfc)) SDL_UnlockSurface(masksfc);
if(SDL_MUSTLOCK(pixsfc)) SDL_UnlockSurface(pixsfc);
percent = (s*100)/m_numsprites;
gResourceLoader.setPermilage(200+percent);
}
gResourceLoader.setPermilage(300);
if(RawData){ delete[] RawData; RawData = NULL;}
LoadSpecialSprites( gGraphics.getSpriteVec(0) );
for(unsigned int i=1 ; i<4 ; i++)
{
gGraphics.getSpriteVec(i) = gGraphics.getSpriteVec(0);
}
// For the other variant let's exchange some colors
auto &SpriteVecPlayer2 = gGraphics.getSpriteVec(1);
for( unsigned int i = 0 ; i < SpriteVecPlayer2.size() ; i++)
{
auto &sprite = SpriteVecPlayer2[i];
// Red against Purple
sprite.exchangeSpriteColor( 5, 4, 0 );
sprite.exchangeSpriteColor( 13, 12, 0 );
// Yellow against Green
sprite.exchangeSpriteColor( 2, 6, 0 );
sprite.exchangeSpriteColor( 10, 14, 0 );
}
auto &SpriteVecPlayer3 = gGraphics.getSpriteVec(2);
for( auto &sprite : SpriteVecPlayer3)
{
// Red against Green
sprite.exchangeSpriteColor( 2, 4, 0 );
sprite.exchangeSpriteColor( 10, 12, 0 );
// Yellow against Purple
sprite.exchangeSpriteColor( 5, 6, 0 );
sprite.exchangeSpriteColor( 13, 14, 0 );
}
auto &SpriteVecPlayer4 = gGraphics.getSpriteVec(3);
for( auto &sprite : SpriteVecPlayer4)
{
// Red against Yellow
sprite.exchangeSpriteColor( 6, 4, 0 );
sprite.exchangeSpriteColor( 14, 12, 0 );
// Green against Purple
sprite.exchangeSpriteColor( 2, 5, 0 );
sprite.exchangeSpriteColor( 10, 13, 0 );
}
for(unsigned int i=0 ; i<4 ; i++)
{
for(Uint16 s=0 ; s<gGraphics.getSpriteVec(i).size() ; s++)
{
GsSprite &Sprite = gGraphics.getSprite(i,s);
Sprite.optimizeSurface();
percent = (s*50)/m_numsprites;
gResourceLoader.setPermilage(300+percent);
}
}
gResourceLoader.setPermilage(350);
std::set<std::string> filelist;
FileListAdder fileListAdder;
std::string gfxpath = JoinPaths(m_gamepath, "gfx");
GetFileList(filelist, fileListAdder, gfxpath, false, FM_REG);
FilterFilelist(filelist, "sprite");
std::set<std::string>::iterator it = filelist.begin();
int listsize = filelist.size();
for( int c=0 ; it != filelist.end() ; it++, c++ )
{
std::string name=*it;
if(name.find("_") != name.npos)
continue;
int num = getRessourceID(name, "sprite");
if(num < m_numsprites )
{
GsSprite &Sprite = gGraphics.getSprite(0, num);
std::string filename = getResourceFilename("gfx/"+name, m_gamepath, false, true);
Sprite.loadHQSprite(filename);
}
percent = (c*150)/listsize;
gResourceLoader.setPermilage(350+percent);
}
gResourceLoader.setPermilage(500);
for(unsigned int i=0 ; i<4 ; i++)
{
const int NoSprites = gGraphics.getSpriteVec(i).size();
for(Uint16 s=0 ; s<NoSprites ; s++)
{
gGraphics.getSprite(i,s).applyTransparency();
percent = (s*250)/NoSprites;
gResourceLoader.setPermilage(500+percent);
}
}
gResourceLoader.setPermilage(750);
// Now create special sprites, like those for effects and the doors!
DeriveSpecialSprites( gGraphics.getTileMap(1), gGraphics.getSpriteVec(0) );
gResourceLoader.setPermilage(800);
// Here special Effects are applied, only when the option is enabled for it
if(gVideoDriver.getSpecialFXConfig())
ApplySpecialFX();
gResourceLoader.setPermilage(900);
// Apply the sprites for player 2,3 and 4
DerivePlayerSprites( 1,gGraphics.getSpriteVec(1) );
DerivePlayerSprites( 2,gGraphics.getSpriteVec(2) );
DerivePlayerSprites( 3,gGraphics.getSpriteVec(3) );
gResourceLoader.setPermilage(1000);
return true;
}
bool CEGALatch::loadData( std::string &path, short episode, int version, unsigned char *data, bool compresseddata )
{
std::string filename;
byte *RawData;
Uint16 width, height;
SDL_Surface *sfc;
filename = getResourceFilename("egalatch.ck" + itoa(episode), path);
FILE* latchfile = OpenGameFile(filename,"rb");
if(!latchfile)
return false;
RawData = new byte[m_latchplanesize * 4];
// get the data out of the file into the memory, decompressing it if necessary.
if (compresseddata)
{
if (lz_decompress(latchfile, (unsigned char*) RawData))
{
return 1;
}
}
else
{
for(int i=0 ; i<(m_latchplanesize*4) ; i++)
RawData[i] = fgetc(latchfile);
}
fclose(latchfile);
// these are the offsets of the different video planes as
// relative to each other--that is if a pixel in plane1
// is at N, the byte for that same pixel in plane3 will be
// at (N + plane3).
unsigned long plane1, plane2, plane3, plane4;
plane1 = 0;
plane2 = (m_latchplanesize * 1);
plane3 = (m_latchplanesize * 2);
plane4 = (m_latchplanesize * 3);
// ** read the 8x8 tiles **
// set up the getbit() function of CPlanes class
CPlanes Planes(RawData);
Planes.setOffsets(plane1 + m_fontlocation, plane2 + m_fontlocation,
plane3 + m_fontlocation, plane4 + m_fontlocation, 0);
// Load these graphics into the CFont Class of CGfxEngine
// The original vorticon engine only uses one fontmap, but we use another for
// extra icons. For example sliders are in that map
g_pGfxEngine->freeFonts();
g_pGfxEngine->createEmptyFontmaps(3);
g_pGfxEngine->getFont(0).loadinternalFont();
CFont &Font = g_pGfxEngine->getFont(1);
Font.CreateSurface( g_pGfxEngine->Palette.m_Palette, SDL_SWSURFACE );
sfc = Font.getSDLSurface();
g_pGfxEngine->getFont(2).loadAlternateFont();
if(SDL_MUSTLOCK(sfc)) SDL_LockSurface(sfc);
Uint8 *pixel = (Uint8*) sfc->pixels;
SDL_FillRect(sfc, NULL, 0);
for(int p=0;p<4;p++)
Planes.readPlaneofTiles(p, pixel, 16, 8, m_fonttiles);
if(SDL_MUSTLOCK(sfc)) SDL_UnlockSurface(sfc);
// ** read the 16x16 tiles **
Planes.setOffsets(plane1 + m_tiles16location,
plane2 + m_tiles16location,
plane3 + m_tiles16location,
plane4 + m_tiles16location,
0);
g_pGfxEngine->freeTilemap();
g_pGfxEngine->createEmptyTilemap(2);
CTilemap &Tilemap = g_pGfxEngine->getTileMap(1);
Tilemap.CreateSurface( g_pGfxEngine->Palette.m_Palette, SDL_SWSURFACE, m_num16tiles, 4, 13 );
sfc = Tilemap.getSDLSurface();
SDL_FillRect(sfc,NULL, 0);
if(SDL_MUSTLOCK(sfc)) SDL_LockSurface(sfc);
Uint8 *u_pixel = (Uint8*) sfc->pixels;
for(int p=0;p<4;p++)
Planes.readPlaneofTiles(p, u_pixel, 13, 16, m_num16tiles);
if(SDL_MUSTLOCK(sfc)) SDL_UnlockSurface(sfc);
// Load Hi-Colour, VGA, SVGA Tiles into the tilemap
filename = getResourceFilename("gfx/ck" + itoa(episode) + "tiles.bmp", path, false);
if(Tilemap.loadHiresTile(filename))
g_pLogFile->textOut(GREEN, "VGA Bitmap for Tileset has been loaded successfully!");
// Adapt the tilemap to the display, so they are faster blit
Tilemap.optimizeSurface();
// make masked tiles according to it's surfaces
applyMasks();
////////////////////
/// Load Bitmaps ///
////////////////////
Planes.setOffsets(plane1 + m_bitmaplocation,
plane2 + m_bitmaplocation,
plane3 + m_bitmaplocation,
plane4 + m_bitmaplocation,
0);
// decode bitmaps into the BitmapData structure. The bitmaps are
// loaded into one continuous stream of image data, with the bitmaps[]
// array giving pointers to where each bitmap starts within the stream.
for(int p=0 ; p<4 ; p++)
{
for(int b=0 ; b<m_bitmaps ; b++)
{
CBitmap &bitmap = g_pGfxEngine->getBitmap(b);
// this points to the location that we're currently
// decoding bitmap data to
sfc= bitmap.getSDLSurface();
if(SDL_MUSTLOCK(sfc)) SDL_LockSurface(sfc);
Uint8* pixel = (Uint8*) sfc->pixels;
if(p==0)
SDL_FillRect(sfc, NULL, 0);
width = bitmap.getWidth(); height = bitmap.getHeight();
// Now read the raw data
Planes.readPlane(p, pixel, width, height);
if(SDL_MUSTLOCK(sfc)) SDL_UnlockSurface(sfc);
}
}
// optimize the bitmaps and load hq bitmaps if there are some.
for(int b=0 ; b<m_bitmaps ; b++)
{
CBitmap &bitmap = g_pGfxEngine->getBitmap(b);
bitmap.optimizeSurface();
}
std::set<std::string> filelist;
FileListAdder fileListAdder;
std::string gfxpath = JoinPaths(path, "gfx");
GetFileList(filelist, fileListAdder, gfxpath, false, FM_REG);
FilterFilelist(filelist, "bitmap");
std::set<std::string>::iterator it = filelist.begin();
for( ; it != filelist.end() ; it++ )
{
std::string filename=*it;
int num = getRessourceID(filename, "bitmap");
CBitmap &bitmap = g_pGfxEngine->getBitmap(num);
filename = getResourceFilename("gfx/" + filename, path, false);
bitmap.loadHQBitmap(filename);
}
if(RawData){ delete[] RawData; RawData = NULL;}
return true;
}
bool CEGALatch::loadData( const std::string &path,
const short episode,
const int version,
const unsigned char *data,
const bool compresseddata )
{
std::string filename;
byte *RawData;
Uint16 width, height;
SDL_Surface *sfc;
filename = getResourceFilename("egalatch.ck" + itoa(episode), path);
FILE* latchfile = OpenGameFile(filename,"rb");
if(!latchfile)
return false;
RawData = new byte[m_latchplanesize * 4];
// get the data out of the file into the memory, decompressing it if necessary.
if (compresseddata)
{
if (lz_decompress(latchfile, (unsigned char*) RawData))
{
return 1;
}
}
else
{
for(int i=0 ; i<(m_latchplanesize*4) ; i++)
RawData[i] = fgetc(latchfile);
}
fclose(latchfile);
// these are the offsets of the different video planes as
// relative to each other--that is if a pixel in plane1
// is at N, the byte for that same pixel in plane3 will be
// at (N + plane3).
unsigned long plane1, plane2, plane3, plane4;
plane1 = 0;
plane2 = (m_latchplanesize * 1);
plane3 = (m_latchplanesize * 2);
plane4 = (m_latchplanesize * 3);
// ** read the 8x8 tiles **
// set up the getbit() function of CPlanes class
CPlanes Planes(RawData);
Planes.setOffsets(plane1 + m_fontlocation, plane2 + m_fontlocation,
plane3 + m_fontlocation, plane4 + m_fontlocation, 0);
// Load these graphics into the GsFont Class of GsGraphics
// The original vorticon engine only uses one fontmap, but we use another for
// extra icons. For example sliders are in that map
gGraphics.freeFonts();
gGraphics.createEmptyFontmaps(3);
gGraphics.getFont(0).loadinternalFont();
GsFont &Font = gGraphics.getFont(1);
Font.CreateSurface( gGraphics.Palette.m_Palette, SDL_SWSURFACE );
sfc = Font.getSDLSurface();
gGraphics.getFont(2).loadAlternateFont();
if(SDL_MUSTLOCK(sfc)) SDL_LockSurface(sfc);
Uint8 *pixel = (Uint8*) sfc->pixels;
SDL_FillRect(sfc, NULL, 0);
for(int p=0;p<4;p++)
Planes.readPlaneofTiles(p, pixel, 16, 8, m_fonttiles);
if(SDL_MUSTLOCK(sfc)) SDL_UnlockSurface(sfc);
// prepare to ** read the 16x16 tiles **
Planes.setOffsets(plane1 + m_tiles16location,
plane2 + m_tiles16location,
plane3 + m_tiles16location,
plane4 + m_tiles16location,
0);
gGraphics.freeTilemap();
gGraphics.createEmptyTilemaps(2);
loadTilemap(gGraphics.getTileMap(0), Planes, episode, path);
// prepare to ** read the 16x16 tiles **, this is for the second plane.
Planes.setOffsets(plane1 + m_tiles16location,
plane2 + m_tiles16location,
plane3 + m_tiles16location,
plane4 + m_tiles16location,
0);
loadTilemap(gGraphics.getTileMap(1), Planes, episode, path);
gGraphics.getTileMap(0).optimizeSurface();
gGraphics.getTileMap(1).optimizeSurface();
// make masked tiles according to it's surfaces
applyMasks();
////////////////////
/// Load Bitmaps ///
////////////////////
Planes.setOffsets(plane1 + m_bitmaplocation,
plane2 + m_bitmaplocation,
plane3 + m_bitmaplocation,
plane4 + m_bitmaplocation,
0);
// decode bitmaps into the BitmapData structure. The bitmaps are
// loaded into one continuous stream of image data, with the bitmaps[]
// array giving pointers to where each bitmap starts within the stream.
for(int p=0 ; p<4 ; p++)
{
for(int b=0 ; b<m_bitmaps ; b++)
{
GsBitmap &bitmap = gGraphics.getBitmapFromId(b);
// this points to the location that we're currently
// decoding bitmap data to
sfc= bitmap.getSDLSurface();
if(SDL_MUSTLOCK(sfc)) SDL_LockSurface(sfc);
Uint8* pixel = (Uint8*) sfc->pixels;
if(p==0)
SDL_FillRect(sfc, NULL, 0);
width = bitmap.getWidth(); height = bitmap.getHeight();
// Now read the raw data
Planes.readPlane(p, pixel, width, height);
if(SDL_MUSTLOCK(sfc)) SDL_UnlockSurface(sfc);
}
}
std::set<std::string> filelist;
FileListAdder fileListAdder;
std::string gfxpath = JoinPaths(path, "gfx");
GetFileList(filelist, fileListAdder, gfxpath, false, FM_REG);
FilterFilelist(filelist, "bitmap");
std::set<std::string>::iterator it = filelist.begin();
for( ; it != filelist.end() ; it++ )
{
std::string filename=*it;
int num = getRessourceID(filename, "bitmap");
GsBitmap &bitmap = gGraphics.getBitmapFromId(num);
filename = getResourceFilename("gfx/" + filename, path, false);
bitmap.loadHQBitmap(filename);
}
if(RawData){ delete[] RawData; RawData = NULL;}
// Create an intro in case it does not exist yet
std::string fullpath = getResourceFilename("preview.bmp", path, false);
if( fullpath == "" )
{ // Not found create it
fullpath = path + "/preview.bmp";
fullpath = GetWriteFullFileName(fullpath, true);
GsBitmap *pBitmap = gGraphics.getBitmapFromStr("TITLE");
SDL_SaveBMP( pBitmap->getSDLSurface(), fullpath.c_str());
}
return true;
}
int main(int argc, const char *argv[]) try
{
std::vector<Joint> joints;
std::vector<RigidBody> rbs;
std::map<std::string,unsigned int> rbindex;
auto xml = XMLParseFile("./default_hand.chr"); // replace string with whatever model you want to test. uses xml variation of John Ratcliff's easy mesh (ezm) file format.
auto const &skx = xml.child("model").child("skeleton");
for (auto const &b : skx.children)
{
rbindex[b.attribute("name")] = rbs.size();
auto verts = ArrayImport<float3>(b.child("verts").body);
auto tris = calchull(verts, verts.size());
float3 pos = StringTo<float3>(b.attribute("position"));
int parent = (b.hasAttribute("parent")) ? (int)rbindex[b.attribute("parent")] : -1;
rbs.push_back(RigidBody({ Shape(verts,tris) }, pos + ((parent>=0)?rbs[parent].position-rbs[parent].com:float3(0,0,0))));
if (parent>=0)
joints.push_back({ parent, (int)rbs.size() - 1, pos, float3(0,0,0), StringTo<float3>(b.child("jointlimitmin").body), StringTo<float3>(b.child("jointlimitmax").body) });
}
rbscalemass(&rbs[0], 3.0f);
rbscalemass(&rbs[1], 5.0f);
DXWin mywin("DX testing articulated rigged model", { 800,600 });
std::vector<Mesh> meshes;
for (auto &rb : rbs)
{
meshes.push_back(MeshSmoothish(rb.shapes[0].verts, rb.shapes[0].tris)); // 1 shape each is known
rb.damping = 0.8f;
//rb.gravscale = 0;
}
for (auto &joint : joints)
{
rbs[joint.rbi0].ignore.push_back(&rbs[joint.rbi1]);
rbs[joint.rbi1].ignore.push_back(&rbs[joint.rbi0]);
joint.p0 -= rbs[joint.rbi0].com;
joint.p1 -= rbs[joint.rbi1].com;
}
for (auto &ja : joints) for (auto &jb : joints) if (ja.rbi0 == jb.rbi0 && ja.rbi1 != jb.rbi1) // ignore siblings
{
rbs[ja.rbi1].ignore.push_back(&rbs[jb.rbi1]);
rbs[jb.rbi1].ignore.push_back(&rbs[ja.rbi1]);
}
for (auto &ja : joints) for (auto &jb : joints) if (ja.rbi1 == jb.rbi0 ) // ignore grandparents
{
rbs[ja.rbi0].ignore.push_back(&rbs[jb.rbi1]);
rbs[jb.rbi1].ignore.push_back(&rbs[ja.rbi0]);
}
std::vector<float3> groundpoints = { { -5.0f, -5.0f, -5.0f }, { 5.0f, -5.0f, -5.0f }, { 5.0f, 10.0f, -5.0f }, { -5.0f, 10.0f, -5.0f }, { -5.0f, -5.0f, -10.0f }, { 5.0f, -5.0f, -10.0f }, { 5.0f, 10.0f, -10.0f }, { -5.0f, 10.0f, -10.0f } };
Mesh ground = MeshSmoothish(groundpoints, { { 0, 1, 2 }, { 2, 3,0 } });
ground.hack = { 1, 1, 0 ,1};
WingMesh cube_wm = WingMeshCube(0.025f);
auto mesh_cube = MeshFlatShadeTex(cube_wm.verts, WingMeshTris(cube_wm));
mesh_cube.hack = { 0, 1, 0, 1 };
Pose camera = { { 0, -10, 0 }, normalize(float4(1, 0, 0, 1)) };
RigidBody *selected = NULL;
float3 spoint=camera * float3(0,0,-10);
float3 rbpoint;
struct Pin{ float3 w; RigidBody* rb; float3 p; };
std::vector<Pin> pins;
mywin.keyboardfunc = [&](int key, int, int)
{
if (key == 'g') for (auto &rb : rbs) rb.gravscale = 1.0f - rb.gravscale;
if (key == 'p' && selected)
Append<Pin>(pins, { spoint, selected, rbpoint });
};
while (mywin.WindowUp())
{
float3 ray = qrot(camera.orientation, normalize(mywin.MouseVector));
if (!selected)
{
for (auto &rb : rbs)
{
float3 v1 = camera.position + ray*100.0f;
if (auto h=ConvexHitCheck(Planes(rb.shapes[0].verts, rb.shapes[0].tris),rb.pose(),camera.position,v1))
{
v1 = h.impact;
selected = &rb;
spoint = h.impact;
rbpoint = rb.pose().inverse()*h.impact;
}
}
}
spoint = camera.position + ray * length(spoint - camera.position)*powf(1.025f, (float)mywin.mousewheel);
mesh_cube.pose.position = spoint;
if (!mywin.MouseState)
selected = NULL;
std::vector<LimitAngular> angulars;
std::vector<LimitLinear> linears;
for (auto const &joint : joints)
{
Append(linears, ConstrainPositionNailed(&rbs[joint.rbi0], joint.p0, &rbs[joint.rbi1], joint.p1));
Append(angulars, ConstrainAngularRange(&rbs[joint.rbi0], &rbs[joint.rbi1], { 0, 0, 0, 1 }, joint.jointlimitmin, joint.jointlimitmax));
}
if (selected)
Append(linears, ConstrainPositionNailed(NULL, spoint, selected, rbpoint));
for(auto &p:pins)
Append(linears, ConstrainPositionNailed(NULL, p.w,p.rb,p.p));
PhysicsUpdate(Addresses(rbs), linears, angulars, { &groundpoints });
for (unsigned int i = 0; i < rbs.size(); i++)
{
meshes[i].pose = rbs[i].pose();
}
mywin.RenderScene(camera, Append(Addresses(meshes),std::vector<Mesh*>({ &ground, &mesh_cube })));
}
}
catch (std::exception e)
{
MessageBoxA(GetActiveWindow(), e.what(), "FAIL", 0);
return -1;
}
int main(int argc, char *argv[]) try
{
physics_driftmax = 0.0025f;
GLWin glwin("point cloud push interaction");
RSCam dcam;
dcam.Init((argc == 2) ? argv[1] : NULL);
Image<unsigned short> dimage(dcam.dcamera());
glwin.ViewAngle = dcam.fov().y;
float viewdist = 2.0f;
float yaw = 120;
int mousexold = 0;
Mesh mesh;
bool pause = false;
bool debuglines=false;
int center = 0;
bool chains = true;
bool usehull = false;
std::vector<RigidBody*> rigidbodies;
std::vector < std::pair<RigidBody*, RigidBody*>> links;
for (float x = -0.2f; x < 0.2f; x+= 0.07f)
for(float z: {0.350f})
for (float y = -0.2f; y <= 0.2f; y += 0.07f)
{
rigidbodies.push_back(new RigidBody({ AsShape(WingMeshDual(WingMeshCube(0.025f),0.028f)) }, { x,y,z }));
//rigidbodies.push_back(new RigidBody({ AsShape(WingMeshCube(0.025f) ) }, { x,y,z }));
links.push_back({(y > -0.2f)?rigidbodies[rigidbodies.size() - 2]:NULL , rigidbodies.back()});
}
//rigidbodies.push_back(new RigidBody({ AsShape(WingMeshCube(0.05f)) }, { 0,0,0.50f }));
auto seesaw = new RigidBody({ AsShape(WingMeshBox({ 0.20f, 0.015f, 0.05f })) }, { 0,0,0.45f });
rigidbodies.push_back(seesaw);
glwin.keyboardfunc = [&](unsigned char key, int x, int y)->void
{
switch (std::tolower(key))
{
case 'q': case 27: exit(0); break; // 27 is ESC
case ' ': pause = !pause; break;
case 'c': chains = !chains; break;
case 'd': debuglines = !debuglines; break;
case 'h': usehull = !usehull; break;
case 'r': for (auto &rb : rigidbodies) { rb->angular_momentum = rb->linear_momentum = float3(0.0f);rb->pose() = { rb->position_start,rb->orientation_start }; } break;
default: std::cout << "unassigned key (" << (int)key << "): '" << key << "'\n"; break;
}
};
if (dcam.dev->supports_option(rs::option::r200_lr_auto_exposure_enabled))
dcam.dev->set_option(rs::option::r200_lr_auto_exposure_enabled, 1);
while (glwin.WindowUp())
{
if (glwin.MouseState)
{
yaw += glwin.mousepos.x - mousexold;
}
mousexold = glwin.mousepos.x;
viewdist *= powf(1.1f, (float)glwin.mousewheel);
if (!pause)
dimage = dcam.GetDepth();
glPushAttrib(GL_ALL_ATTRIB_BITS);
glViewport(0, 0, glwin.res.x, glwin.res.y);
glClearColor(0.1f, 0.1f, 0.15f, 1);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
gluPerspective(glwin.ViewAngle, (double)glwin.aspect_ratio(), 0.01f, 50.0f);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
gluLookAt(0, 0, viewdist, 0, 0, 0, 0, -1, 0);
glEnable(GL_DEPTH_TEST);
glTranslatef(0, 0, 0.35f);
glRotatef(yaw, 0, 1, 0);
glTranslatef(0, 0, -0.35f);
std::vector<float3> pts;
std::vector<float3> outliers;
std::vector<float3> vpts;
glDisable(GL_BLEND);
float2 wrange = { 0.20f,0.60f };
auto dp_image_c = Transform(dimage, [](unsigned short s) {return byte3((unsigned char)clamp(255 - s / 4, 0, 255)); });
drawimage(dp_image_c, { 0.78f,0.22f }, { 0.2f,-0.2f }, 3);
float depth_scale = (dcam.dev) ? dcam.dev->get_depth_scale() : 0.001f; // to put into meters // if file assume file is mm
for (auto p : rect_iteration(dimage.dim())) // p is int2 from 0,0 to w,h of dcam
{
float d = dimage.pixel(p) * depth_scale; // d is in meters, whereas depth[i] is in camera units mm for R200, .125mm for SR300 ivycam
if (p.x<5 || p.x> dimage.dim().x - 5 || p.y<5 || p.y>dimage.dim().y - 5) continue; // crop, seems to be lots of noise at the edges
if (d > 1.0f) // just too far
continue;
float3 v = dimage.cam.deprojectz(asfloat2(p), d);
if (d>wrange.x && d < wrange.y)
pts.push_back(v);
else
outliers.push_back(v);
}
vpts = ObtainVoxelPointCloud(pts, 0.0082f, 8);
std::vector<std::pair<float3, float3>> lines;
std::vector<std::pair<float3, float3>> glines;
if (1)// && pts.size())
{
std::vector<LimitLinear> linears;
std::vector<LimitAngular> angulars;
physics_gravity = { 0, (float) chains,0 }; // ugg y is down
if(!usehull) for(auto rb:rigidbodies)
{
if (!rb->shapes[0].planes.size())
rb->shapes[0].planes = Planes(rb->shapes[0].verts, rb->shapes[0].tris);
auto planes = Transform(rb->shapes[0].planes, [&](float4 p) { return rb->pose().TransformPlane(p);});
rb->gravscale = (float)chains;
float separation = FLT_MAX;
float3 pushpoint = float3(0, 0, 0); //
float4 pushplane;
for (auto p : vpts)
{
auto plane = mostabove(planes, p);
float sep;
if ((sep = dot(plane, float4(p, 1))) < separation)
{
pushpoint = p;
pushplane = plane;
separation = sep;
}
}
if (separation > 0.1f)
continue;
float3 closestpoint = ProjectOntoPlane(pushplane, pushpoint);
pushplane = float4({ -pushplane.xyz(), -dot(-pushplane.xyz(),pushpoint) });
linears.push_back(ConstrainAlongDirection(NULL, pushpoint, rb, rb->pose().inverse()*closestpoint, pushplane.xyz(), 0, 100.0f)); // FLT_MAX));
lines.push_back({ closestpoint,pushpoint });
auto cp=Separated(rb->shapes[0].verts, rb->position, rb->orientation, { pushpoint }, { 0,0,0 }, { 0,0,0,1 }, 1);
glines.push_back({ cp.p0w, cp.p1w });
}
Append(linears, ConstrainPositionNailed(NULL, seesaw->position_start, seesaw, { 0, 0, 0 }));
Append(angulars, ConstrainAngularRange(NULL, seesaw, { 0, 0, 0, 1 }, { 0, 0,-20 }, { 0, 0,20 }));
if (chains) for (auto link : links)
Append(linears, ConstrainPositionNailed(link.first,link.first? float3(0, 0.035f, 0) : link.second->position_start-float3(0, -0.035f, 0) , link.second, { 0,-0.035f,0 }));
if(!pause)
if(usehull && vpts.size()>5)
PhysicsUpdate(rigidbodies, linears, angulars, { &vpts });
else
PhysicsUpdate(rigidbodies, linears, angulars, std::vector<std::vector<float3>*>());
}
glColor3f(1, 1, 1);
glwirefrustumz(dcam.deprojectextents(), { 0.1f,1.0f }); // draw the camera frustum volume
glPushAttrib(GL_ALL_ATTRIB_BITS);
glPointSize(1);
glBegin(GL_POINTS);
glColor3f(0, 1, 0.5f);
for (auto p : pts)
glVertex3fv(p);
glColor3f(1, 0.15f, 0.15f);
for (auto p : outliers)
glVertex3fv(p);
glEnd();
glPointSize(3);
glBegin(GL_POINTS);
glColor3f(1, 1, 1);
for (auto p : vpts) // was: spts
glVertex3fv(p);
glEnd();
glPopAttrib();
if (debuglines)
{
glBegin(GL_LINES);
glColor3f(0, 1, 1);
if (0)for (auto line : lines)
glVertex3fv(line.first), glVertex3fv(line.second);
glColor3f(1, 1, 0);
for (auto line : glines)
glVertex3fv(line.first), glVertex3fv(line.second);
glEnd();
}
if (usehull && vpts.size() > 5)
{
auto tris = calchull(vpts, 0);
glBegin(GL_LINES);
glColor3f(1, 1, 1);
for (auto t : tris) for( int i : {0,1,1,2,2,0})
glVertex3fv(vpts[t[i]]);
glEnd();
}
if (chains)
{
glBegin(GL_LINES);
glColor3f(1, 0, 1);
for (auto link : links)
{
if(link.first)
glVertex3fv(link.first->pose()* float3(0, 0, 0)), glVertex3fv(link.first->pose()* float3(0, 0.035f, 0));
glVertex3fv(link.second->pose()* float3(0, 0, 0)) , glVertex3fv(link.second->pose()* float3(0, -0.035f, 0));
}
glEnd();
}
glPushAttrib(GL_ALL_ATTRIB_BITS);
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
glEnable(GL_TEXTURE_2D);
glColor3f(0.5f, 0.5f, 0.5f);
for (auto &rb : rigidbodies)
rbdraw(rb);
glPopAttrib(); // Restore state
// Restore state
glPopMatrix(); //should be currently in modelview mode
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glPopAttrib();
glMatrixMode(GL_MODELVIEW);
glwin.PrintString({ 0,0 }, "esc to quit.");
glwin.PrintString({ 0,1 }, "[h] collision %s ",(usehull)?"hull":"points");
glwin.SwapBuffers();
}
return 0;
}
catch (const char *c)
{
MessageBox(GetActiveWindow(), c, "FAIL", 0);
}
catch (std::exception e)
{
MessageBox(GetActiveWindow(), e.what(), "FAIL", 0);
}