//TT -- Changed to include height and color
// void AddWall(int row, int col)
// {
// AddWall(row, col, DefaultHeight, BasicColor.White);
// }
// Adds a simple cube at a specified location in the maze grid
void AddWall(int row, int col, float height, BasicColor color)
{
// TT Oct-2006 - Add an option to use a sphere instead of a cube
if (_UseSphere[(byte)color])
{
SphereShapeProperties cSphereShape = null;
SingleShapeEntity sphere = null;
float radius;
radius = _state.SphereScale * height / 2.0f;
// create simple entity, with a single shape
cSphereShape = new SphereShapeProperties(
// TT -- Change to infinite mass so that the walls
// do not "blow apart" if you bump them!
// TT Oct 2006 -- Allow user to specify this
_WallMasses[(byte)color], // mass in kilograms.
new Pose(), // relative pose
radius); // radius
cSphereShape.Material = new MaterialProperties("gsphere", 1.0f, 0.01f, 0.01f);
// Set the color of the sphere according to the bitmap image
// or the specified color if no bitmap
if (_WallTextures[(byte)color] == "")
{
// TT - Changed for October CTP because DiffuseColor
// is a Vector4, but my WallColors are Vector3
//cBoxShape.DiffuseColor = _WallColors[(byte)color];
cSphereShape.DiffuseColor.X = (float)(_WallColors[(byte)color].X / 255.0);
cSphereShape.DiffuseColor.Y = (float)(_WallColors[(byte)color].Y / 255.0);
cSphereShape.DiffuseColor.Z = (float)(_WallColors[(byte)color].Z / 255.0);
cSphereShape.DiffuseColor.W = 1.0f;
}
else
cSphereShape.TextureFileName = _WallTextures[(byte)color];
sphere = new SingleShapeEntity(new SphereShape(cSphereShape),
new Vector3((col * -_state.GridSpacing),
radius,
-(row * _state.GridSpacing)));
// Name the entity. All entities must have unique names
sphere.State.Name = "ball_" + row + "_" + col;
// Insert entity in simulation.
_simEnginePort.Insert(sphere);
}
else
{
// Dimensions are in meters
Vector3 dimensions =
new Vector3(_state.WallBoxSize * _state.GridSpacing,
height * _state.HeightScale,
_state.WallBoxSize * _state.GridSpacing);
BoxShapeProperties cBoxShape = null;
SingleShapeEntity box = null;
// create simple immovable entity, with a single shape
cBoxShape = new BoxShapeProperties(
// TT -- Change to infinite mass so that the walls
// do not "blow apart" if you bump them!
// TT Oct 2006 -- Allow user to specify this
_WallMasses[(byte)color], // mass in kilograms.
new Pose(), // relative pose
dimensions); // dimensions
cBoxShape.Material = new MaterialProperties("gbox", 1.0f, 0.4f, 0.5f);
// Set the color of the box according to the bitmap image
// or the specified color if no bitmap
if (_WallTextures[(byte)color] == "")
{
// TT - Changed for October CTP because DiffuseColor
// is a Vector4, but my WallColors are Vector3
//cBoxShape.DiffuseColor = _WallColors[(byte)color];
cBoxShape.DiffuseColor.X = (float)(_WallColors[(byte)color].X / 255.0);
cBoxShape.DiffuseColor.Y = (float)(_WallColors[(byte)color].Y / 255.0);
cBoxShape.DiffuseColor.Z = (float)(_WallColors[(byte)color].Z / 255.0);
cBoxShape.DiffuseColor.W = 0.5f;
}
else
cBoxShape.TextureFileName = _WallTextures[(byte)color];
box = new SingleShapeEntity(new BoxShape(cBoxShape),
new Vector3(col * -_state.GridSpacing,
height * _state.HeightScale / 2,
-(row * _state.GridSpacing)));
// Name the entity. All entities must have unique names
box.State.Name = "wall_" + row + "_" + col;
// Insert entity in simulation.
_simEnginePort.Insert(box);
}
BlockCounter++;
}
// TT -- Changed to add a height and color
// void AddWall(int startRow, int startCol, int endRow, int endCol)
// {
// AddWall(startRow, startCol, endRow, endCol, DefaultHeight);
// }
// Adds a long wall in the maze grid
// Useful for reducing number of elements in simulation for better performance
// TT -- Note that the existing code used height to refer to the
// depth of the wall. Therefore the real height is called boxSize.
void AddWall(int startRow, int startCol, int endRow, int endCol, float boxSize, BasicColor color)
{
int width = Math.Abs(endCol - startCol) + 1;
int height = Math.Abs(endRow - startRow) + 1;
float realWidth = (width * _state.GridSpacing) - (_state.GridSpacing - _state.WallBoxSize*_state.GridSpacing);
float realHeight = (height * _state.GridSpacing) - (_state.GridSpacing - _state.WallBoxSize*_state.GridSpacing);
//because the box is placed relative to the center of mass
float widthOffset = (Math.Abs(endCol - startCol) * _state.GridSpacing) / 2;
float heightOffset = -(Math.Abs(endRow - startRow) * _state.GridSpacing) / 2;
if (_UseSphere[(byte)color])
{
// This object is a Sphere
SphereShapeProperties cSphereShape = null;
SingleShapeEntity sphere = null;
float radius;
radius = (float)(_state.SphereScale * Math.Sqrt(realWidth * realWidth + realHeight * realHeight) / 2.0f);
// create simple entity, with a single shape
cSphereShape = new SphereShapeProperties(
// TT -- Change to infinite mass so that the walls
// do not "blow apart" if you bump them!
// TT Oct 2006 -- Allow user to specify this
_WallMasses[(byte)color], // mass in kilograms.
new Pose(), // relative pose
radius); // radius
cSphereShape.Material = new MaterialProperties("gsphere", 0.9f, 0.05f, 0.1f);
cSphereShape.Material.Advanced = new MaterialAdvancedProperties();
cSphereShape.Material.Advanced.RestitutionCombineMode = CoefficientsCombineMode.Max;
cSphereShape.Material.Advanced.FrictionCombineMode = CoefficientsCombineMode.Min;
cSphereShape.Material.Advanced.Spring = new SpringProperties();
cSphereShape.Material.Advanced.Spring.SpringCoefficient = 0.9f;
cSphereShape.Material.Advanced.Spring.DamperCoefficient = 0.1f;
cSphereShape.Advanced = new ShapeAdvancedProperties();
cSphereShape.Advanced.PhysicsCalculationPasses = 20.0f;
//cSphereShape.Advanced.MassSpaceIntertiaTensor = new Vector3(1.0f, 0.0f, 1.0f);
// TT - These do not seem to have much effect
//cSphereShape.MassDensity.AngularDamping = 0.0f;
//cSphereShape.MassDensity.LinearDamping = 0.0f;
//cSphereShape.MassDensity.Mass = _WallMasses[(byte)color];
//cSphereShape.MassDensity.Density = 0.1f;
// Set the color of the sphere according to the bitmap image
// or the specified color if no bitmap
if (_WallTextures[(byte)color] == "")
{
// TT - Changed for October CTP because DiffuseColor
// is a Vector4, but my WallColors are Vector3
//cBoxShape.DiffuseColor = _WallColors[(byte)color];
cSphereShape.DiffuseColor.X = (float)(_WallColors[(byte)color].X / 255.0);
cSphereShape.DiffuseColor.Y = (float)(_WallColors[(byte)color].Y / 255.0);
cSphereShape.DiffuseColor.Z = (float)(_WallColors[(byte)color].Z / 255.0);
cSphereShape.DiffuseColor.W = 1.0f;
}
else
cSphereShape.TextureFileName = _WallTextures[(byte)color];
sphere = new SingleShapeEntity(new SphereShape(cSphereShape),
new Vector3((startCol * -_state.GridSpacing) - widthOffset,
radius*2,
-(startRow * _state.GridSpacing) + heightOffset)
);
// Name the entity. All entities must have unique names
sphere.State.Name = "ball_" + startRow + "_" + startCol;
// Insert entity in simulation.
_simEnginePort.Insert(sphere);
/*
SphereShapeProperties cSphereShape = null;
SingleShapeEntity sphere = null;
float radius;
radius = (float)(_state.SphereScale * Math.Sqrt(realWidth * realWidth + realHeight * realHeight) / 2.0f);
// create simple entity, with a single shape
cSphereShape = new SphereShapeProperties(
1.0f, // mass in kilograms.
new Pose(), // relative pose
radius); // radius
cSphereShape.Material = new MaterialProperties("gsphere", 1.0f, 0.01f, 0.01f);
sphere = new SingleShapeEntity(new SphereShape(cSphereShape),
new Vector3((startCol * -_state.GridSpacing) - widthOffset,
radius,
-(startRow * _state.GridSpacing) + heightOffset)
);
// Name the entity. All entities must have unique names
sphere.State.Name = "ball_" + startRow + "_" + startCol;
// Insert entity in simulation.
_simEnginePort.Insert(sphere);
*/
}
else
{
// This object is a wall (stretched cube)
Vector3 dimensions =
new Vector3(realWidth, boxSize * _state.HeightScale, realHeight);
// Dimensions are in meters
BoxShapeProperties cBoxShape = null;
SingleShapeEntity box = null;
cBoxShape = new BoxShapeProperties(
// TT -- Change to infinite mass so that the walls
// do not "blow apart" if you bump them!
// TT Oct 2006 -- Allow user to specify this
_WallMasses[(byte)color], // mass in kilograms.
new Pose(), // relative pose
dimensions); // dimensions
// cBoxShape = new BoxShapeProperties(0, new Pose(), dimensions);
// Walls have the same properties as the ground
cBoxShape.Material = new MaterialProperties("gbox", 0.8f, 0.5f, 0.8f);
// Set the color of the box according to the bitmap image
// or the specified color if no bitmap
if (_WallTextures[(byte)color] == "")
{
// TT - Changed for October CTP because DiffuseColor
// is a Vector4, but my WallColors are Vector3
//cBoxShape.DiffuseColor = _WallColors[(byte)color];
cBoxShape.DiffuseColor.X = (float)(_WallColors[(byte)color].X / 255.0);
cBoxShape.DiffuseColor.Y = (float)(_WallColors[(byte)color].Y / 255.0);
cBoxShape.DiffuseColor.Z = (float)(_WallColors[(byte)color].Z / 255.0);
cBoxShape.DiffuseColor.W = 0.5f;
}
else
cBoxShape.TextureFileName = _WallTextures[(byte)color];
// Raul
if (color == BasicColor.White)
{
cBoxShape.TextureFileName = "bricks_4.jpg";
}
if (color == BasicColor.Red)
{
cBoxShape.TextureFileName = "wood2.jpg";
}
box = new SingleShapeEntity(new BoxShape(cBoxShape),
new Vector3((startCol * -_state.GridSpacing) - widthOffset,
boxSize * _state.HeightScale / 2,
-(startRow * _state.GridSpacing) + heightOffset)
);
// Name the entity. All entities must have unique names
box.State.Name = "wall_" + startRow + "_" + startCol;
_simEnginePort.Insert(box);
}
BlockCounter++;
}
private void PopulateWorld()
{
AddSky();
AddGround();
// AddBlocks();
// Add an overhead camera
// AddCamera();
// Create and place the dominos
// SpawnIterator(CreateDominos);
// Create a LynxL6Arm Entity positioned at the origin
var robotArm = new SimulatedRobotArmEntity(RobotArmEntityName, new Vector3(0, 0, 0));
SimulationEngine.GlobalInstancePort.Insert(robotArm);
var targetProps = new SphereShapeProperties(0, new Pose(), 0.0025f);
var shape = new SphereShape(targetProps);
shape.State.DiffuseColor = new Vector4(0.1f, 0f, 1f, 1f);
_moveTargetEntity = new SingleShapeEntity(shape, new Vector3(0f, 0.2f, 0.1f));
_moveTargetEntity.State.Name = "Move To Target";
SimulationEngine.GlobalInstancePort.Insert(_moveTargetEntity);
}
