/// <summary>
/// Creates a new propeller entity
/// </summary>
/// <param name="name">Name of the entity</param>
/// <param name="position">The position of the propeller to be placed</param>
public PropellerEntity(string name, Vector3 position, Quaternion orientation, Vector3 forceVector)
{
// Apply the entity from the given parameter
State.Name = name;
localPosition = position;
this.forceDirectionVector = forceVector;
// Create global objects
meshRotationVector = new Vector3();
// Load and scale the 3D mesh of the multicopter
//Flags |= VisualEntityProperties.UsesAlphaBlending;
//Flags |= VisualEntityProperties.DisableBackfaceCulling;
// Apply a box shape for the propeller
propellerShapeProperties = new BoxShapeProperties(
"BoxShapeProperties " + name,
PROPELLER_MASS,
new Pose(localPosition),
PROPELLER_BOXSHAPE_DIMENSION
);
propellerShapeProperties.LocalPose.Position = localPosition;
propellerShapeProperties.LocalPose.Orientation = orientation;
}
/// <summary>
/// Custom constructor for building model from hardcoded values. Used to create entity programmatically
/// </summary>
/// <param name="initialPos"></param>
public ScribblerRobot(Vector3 initialPos)
{
MASS = 0.68f; //kg (1.5 lbs)
// the default settings approximate the Scribbler chassis
CHASSIS_DIMENSIONS = new Vector3(0.1524f, //meters wide (6 in)
0.0508f, //meters high (2 in)
0.1905f); //meters long (7.5 in)
FRONT_WHEEL_MASS = 0.01f;
CHASSIS_CLEARANCE = 0.006f; //0.25 in
FRONT_WHEEL_RADIUS = 0.0381f; //1.5 in
CASTER_WHEEL_RADIUS = 0.0127f; //0.5 in
FRONT_WHEEL_WIDTH = 0.006f; //0.25 in
//CASTER_WHEEL_WIDTH = 0.008f; //not currently used
FRONT_AXLE_DEPTH_OFFSET = 0; // distance of the axle from the center of robot //3.75 in
base.State.Name = "ScribblerRobot";
base.State.MassDensity.Mass = MASS;
base.State.Pose.Position = initialPos;
//this is because the robot is hevily back weighted
base.State.MassDensity.CenterOfMass = new Pose(new Vector3(0f, 0.025f, 0.025f));
//todo
// reference point for all shapes is the projection of
// the center of mass onto the ground plane
// (basically the spot under the center of mass, at Y = 0, or ground level)
// NOTE: right/left is from the perspective of the robot, looking forward
// NOTE: X = width of robot (right to left), Y = height, Z = length
// chassis position
BoxShapeProperties motorBaseDesc = new BoxShapeProperties("Scribbler Body", MASS,
new Pose(new Vector3(
0, // Chassis center is also the robot center, so use zero for the X axis offset
CHASSIS_CLEARANCE + CHASSIS_DIMENSIONS.Y / 2, // chassis is off the ground and its center is DIM.Y/2 above the clearance
0)), // any offset in the z/length axis
CHASSIS_DIMENSIONS);
motorBaseDesc.Material = new MaterialProperties("high friction", 0.0f, 1.0f, 20.0f);
motorBaseDesc.Name = "Chassis";
ChassisShape = new BoxShape(motorBaseDesc);
// rear wheel is also called the caster
CASTER_WHEEL_POSITION = new Vector3(0, // center of chassis widthwise
CASTER_WHEEL_RADIUS, // distance from ground
CHASSIS_DIMENSIONS.Z / 2 - CASTER_WHEEL_RADIUS / 2); // all the way at the back of the robot
RIGHT_FRONT_WHEEL_POSITION = new Vector3(
+CHASSIS_DIMENSIONS.X / 2 - FRONT_WHEEL_WIDTH / 2, // left of center
FRONT_WHEEL_RADIUS, // distance from ground of axle
FRONT_AXLE_DEPTH_OFFSET); // distance from center, on the z-axis
LEFT_FRONT_WHEEL_POSITION = new Vector3(
-CHASSIS_DIMENSIONS.X / 2 - FRONT_WHEEL_WIDTH / 2, // right of center
FRONT_WHEEL_RADIUS, // distance from ground of axle
FRONT_AXLE_DEPTH_OFFSET); // distance from center, on the z-axis
MotorTorqueScaling = 10;
}
private void AddArena()
{
BoxShapeProperties tBoxShape = null;
SingleShapeEntity tBoxEntity = null;
tBoxShape = new BoxShapeProperties(500f, new Pose(), new Vector3(5f, 0.25f, 0.25f));
tBoxEntity = new SingleShapeEntity(new BoxShape(tBoxShape), new Vector3(0f, 0.5f, 2.63f));
tBoxEntity.State.Name = "arena top";
SimulationEngine.GlobalInstancePort.Insert(tBoxEntity);
BoxShapeProperties lBoxShape = null;
SingleShapeEntity lBoxEntity = null;
lBoxShape = new BoxShapeProperties(500f, new Pose(), new Vector3(0.25f, 0.25f, 5f));
lBoxEntity = new SingleShapeEntity(new BoxShape(lBoxShape), new Vector3(-2.63f, 0.5f, 0f));
lBoxEntity.State.Name = "arena left";
SimulationEngine.GlobalInstancePort.Insert(lBoxEntity);
BoxShapeProperties rBoxShape = null;
SingleShapeEntity rBoxEntity = null;
rBoxShape = new BoxShapeProperties(500f, new Pose(), new Vector3(0.25f, 0.25f, 5f));
rBoxEntity = new SingleShapeEntity(new BoxShape(rBoxShape), new Vector3(2.63f, 0.5f, 0f));
rBoxEntity.State.Name = "arena right";
SimulationEngine.GlobalInstancePort.Insert(rBoxEntity);
BoxShapeProperties bBoxShape = null;
SingleShapeEntity bBoxEntity = null;
bBoxShape = new BoxShapeProperties(500f, new Pose(), new Vector3(5f, 0.25f, 0.25f));
bBoxEntity = new SingleShapeEntity(new BoxShape(bBoxShape), new Vector3(0f, 0.5f, -2.63f));
bBoxEntity.State.Name = "arena bottom";
SimulationEngine.GlobalInstancePort.Insert(bBoxEntity);
}
private void AddCube(Vector3 position)
{
BoxShapeProperties cBoxShape = null;
SingleShapeEntity cBoxEntity = null;
cBoxShape = new BoxShapeProperties(2.0f, new Pose(), new Vector3(0.5f, 0.5f, 0.5f));
cBoxEntity = new SingleShapeEntity(new BoxShape(cBoxShape), position);
cBoxEntity.State.Name = "cube:" + Guid.NewGuid().ToString();
SimulationEngine.GlobalInstancePort.Insert(cBoxEntity);
}
/// <summary>
/// Initialization constructor
/// </summary>
/// <param name="initialPos"></param>
public MotorBase(Vector3 initialPos)
{
MASS = 9;
CHASSIS_DIMENSIONS = new Vector3(0.393f, 0.18f, 0.40f);
CHASSIS_CLEARANCE = 0.05f;
FRONT_WHEEL_RADIUS = 0.08f;
CASTER_WHEEL_RADIUS = 0.025f; // = CHASSIS_CLEARANCE / 2; // to keep things simple we make caster a bit bigger
FRONT_WHEEL_WIDTH = 4.74f; //not used
CASTER_WHEEL_WIDTH = 0.02f; //not used
FRONT_AXLE_DEPTH_OFFSET = -0.05f; // distance of the axle from the center of robot
base.State.Name = "MotorBaseWithThreeWheels";
base.State.MassDensity.Mass = MASS;
base.State.Pose.Position = initialPos;
// reference point for all shapes is the projection of
// the center of mass onto the ground plane
// (basically the spot under the center of mass, at Y = 0, or ground level)
// chassis position
BoxShapeProperties motorBaseDesc = new BoxShapeProperties("chassis", MASS,
new Pose(new Vector3(
0, // Chassis center is also the robot center, so use zero for the X axis offset
CHASSIS_CLEARANCE + CHASSIS_DIMENSIONS.Y / 2, // chassis is off the ground and its center is DIM.Y/2 above the clearance
0)), // no offset in the z/depth axis, since again, its center is the robot center
CHASSIS_DIMENSIONS);
motorBaseDesc.Material = new MaterialProperties("high friction", 0.0f, 1.0f, 20.0f);
motorBaseDesc.Name = "Chassis";
ChassisShape = new BoxShape(motorBaseDesc);
// rear wheel is also called the caster
CASTER_WHEEL_POSITION = new Vector3(0, // center of chassis
CASTER_WHEEL_RADIUS, // distance from ground
CHASSIS_DIMENSIONS.Z / 2 - CASTER_WHEEL_RADIUS); // all the way at the back of the robot
// NOTE: right/left is from the perspective of the robot, looking forward
FRONT_WHEEL_MASS = 0.10f;
RIGHT_FRONT_WHEEL_POSITION = new Vector3(
CHASSIS_DIMENSIONS.X / 2 + 0.01f - 0.05f, // left of center
FRONT_WHEEL_RADIUS, // distance from ground of axle
FRONT_AXLE_DEPTH_OFFSET); // distance from center, on the z-axis
LEFT_FRONT_WHEEL_POSITION = new Vector3(
-CHASSIS_DIMENSIONS.X / 2 - 0.01f + 0.05f, // right of center
FRONT_WHEEL_RADIUS, // distance from ground of axle
FRONT_AXLE_DEPTH_OFFSET); // distance from center, on the z-axis
MotorTorqueScaling = 20;
ConstructStateMembers();
}
/// <summary>
/// Initialization constructor
/// </summary>
/// <param name="localPose"></param>
public SonarEntity(Pose localPose)
{
// create a new instance of the sonar pose so we dont re-use the raycast reference
// That reference will be updated regularly
BoxShapeProperties box = new BoxShapeProperties("Sonar", 0.5f,
localPose,
new Vector3(0.1f, 0.1f, 0.1f));
_sonarBox = new BoxShape(box);
State.Assets.Effect = "LaserRangeFinder.fx";
}
public void AddBoundaries(float dimX, float dimY)
{
//Right Wall
Vector3 dimensions = new Vector3(.5f, .5f, dimY);
Vector3 position = new Vector3(dimX, .5f, -(dimY / 2f));
BoxShapeProperties tBoxShape = new BoxShapeProperties(10000000f, new Pose(), dimensions);
tBoxShape.Material = new MaterialProperties("tbox", 0.4f, 0.4f, 0.6f);
tBoxShape.DiffuseColor = new Vector4(0.5f, 0.5f, 0.5f, 1.0f);
SingleShapeEntity tBoxEntity = new SingleShapeEntity(new BoxShape(tBoxShape), position); ;
tBoxEntity.State.Assets.DefaultTexture = "env2.bmp";
tBoxEntity.State.Name = "Wall";
//Top Wall
Vector3 dimensions2 = new Vector3(dimX - 1, .5f, .5f);
Vector3 position2 = new Vector3((dimX / 2f), .5f, 0);
BoxShapeProperties tBoxShape2 = new BoxShapeProperties(10000000f, new Pose(), dimensions2);
tBoxShape2.Material = new MaterialProperties("tbox", 0.4f, 0.4f, 0.6f);
tBoxShape2.DiffuseColor = new Vector4(0.5f, 0.5f, 0.5f, 1.0f);
SingleShapeEntity tBoxEntity2 = new SingleShapeEntity(new BoxShape(tBoxShape2), position2); ;
tBoxEntity2.State.Assets.DefaultTexture = "env2.bmp";
tBoxEntity2.State.Name = "Wall2";
//Bottom Wall
Vector3 dimensions3 = new Vector3(dimX - 1, .5f, .5f);
Vector3 position3 = new Vector3((dimX / 2f), .5f, -dimY);
BoxShapeProperties tBoxShape3 = new BoxShapeProperties(10000000f, new Pose(), dimensions3);
tBoxShape3.Material = new MaterialProperties("tbox", 0.4f, 0.4f, 0.6f);
tBoxShape3.DiffuseColor = new Vector4(0.5f, 0.5f, 0.5f, 1.0f);
SingleShapeEntity tBoxEntity3 = new SingleShapeEntity(new BoxShape(tBoxShape3), position3); ;
tBoxEntity3.State.Assets.DefaultTexture = "env2.bmp";
tBoxEntity3.State.Name = "Wall3";
//Left Wall
Vector3 dimensions4 = new Vector3(.5f, .5f, dimY);
Vector3 position4 = new Vector3(0, .5f, -(dimY / 2f));
BoxShapeProperties tBoxShape4 = new BoxShapeProperties(10000000f, new Pose(), dimensions4);
tBoxShape4.Material = new MaterialProperties("tbox", 0.4f, 0.4f, 0.6f);
tBoxShape4.DiffuseColor = new Vector4(0.5f, 0.5f, 0.5f, 1.0f);
SingleShapeEntity tBoxEntity4 = new SingleShapeEntity(new BoxShape(tBoxShape4), position4); ;
tBoxEntity4.State.Assets.DefaultTexture = "env2.bmp";
tBoxEntity4.State.Name = "Wall4";
SimulationEngine.GlobalInstancePort.Insert(tBoxEntity);
SimulationEngine.GlobalInstancePort.Insert(tBoxEntity2);
SimulationEngine.GlobalInstancePort.Insert(tBoxEntity3);
SimulationEngine.GlobalInstancePort.Insert(tBoxEntity4);
this.BoundaryList.Add(tBoxEntity);
this.BoundaryList.Add(tBoxEntity2);
this.BoundaryList.Add(tBoxEntity3);
this.BoundaryList.Add(tBoxEntity4);
}
/// <summary>
/// Custom constructor for building model from hardcoded values. Used to create entity programmatically
/// </summary>
/// <param name="initialPos"></param>
public BoeBot(Vector3 initialPos)
{
MASS = 0.454f; //质量
// the default settings approximate the BoeBot chassis
CHASSIS_DIMENSIONS = new Vector3(0.09f, //宽
0.09f, //高
0.13f); //长
FRONT_WHEEL_MASS = 0.01f;
CHASSIS_CLEARANCE = 0.015f;
FRONT_WHEEL_RADIUS = 0.025f;
CASTER_WHEEL_RADIUS = 0.0125f;
FRONT_WHEEL_WIDTH = 0.01f;
CASTER_WHEEL_WIDTH = 0.008f; //not currently used, but dim is accurate
FRONT_AXLE_DEPTH_OFFSET = 0.01f; // distance of the axle from the center of robot
base.State.Name = "BoeBot";
base.State.MassDensity.Mass = MASS;
base.State.Pose.Position = initialPos;
BoxShapeProperties motorBaseDesc = new BoxShapeProperties("BoeBot Body", MASS,
new Pose(new Vector3(
0, // Chassis center is also the robot center, so use zero for the X axis offset
CHASSIS_CLEARANCE + CHASSIS_DIMENSIONS.Y / 2, // chassis is off the ground and its center is DIM.Y/2 above the clearance
0.03f)), // minor offset in the z/depth axis
CHASSIS_DIMENSIONS);
motorBaseDesc.Material = new MaterialProperties("high friction", 0.0f, 1.0f, 20.0f);
motorBaseDesc.Name = "Chassis";
ChassisShape = new BoxShape(motorBaseDesc);
// rear wheel is also called the caster
CASTER_WHEEL_POSITION = new Vector3(0, // center of chassis
CASTER_WHEEL_RADIUS, // distance from ground
CHASSIS_DIMENSIONS.Z / 2); // at the rear of the robot
RIGHT_FRONT_WHEEL_POSITION = new Vector3(
+CHASSIS_DIMENSIONS.X / 2, // left of center
FRONT_WHEEL_RADIUS, // distance from ground of axle
FRONT_AXLE_DEPTH_OFFSET); // distance from center, on the z-axis
LEFT_FRONT_WHEEL_POSITION = new Vector3(
-CHASSIS_DIMENSIONS.X / 2, // right of center
FRONT_WHEEL_RADIUS, // distance from ground of axle
FRONT_AXLE_DEPTH_OFFSET); // distance from center, on the z-axis
MotorTorqueScaling = 30;
// specify a default mesh
State.Assets.Mesh = "boe-bot.bos";
}
/// <summary>
/// Build the the physical model
/// </summary>
/// <param name="device">A reference to the used graphics graphics device</param>
/// <param name="physicsEngine">A reference to the used physics engine</param>
private void ProgrammaticallyBuildModel(xnagrfx.GraphicsDevice device, PhysicsEngine physicsEngine)
{
// Create central box
BoxShapeProperties centerBoxChassisProperties = new BoxShapeProperties(
0.2f, // 200g
new Pose(new Vector3(0, CHASSIS_DIMENSIONS_HEIGHT / 2, 0)),
new Vector3(0.06f, CHASSIS_DIMENSIONS_HEIGHT, 0.06f)
);
//State.PhysicsPrimitives.Add(new BoxShape(centerBoxChassisProperties));
// Create box shape dynamically for each multicopter arm
float angleOffset = (2 * xna.MathHelper.Pi) / Globals.Instance.multicopterPropellerCount[multicopterType];
Vector3 multicopterCenter = new Vector3(ARM_CHASSIS_DIMENSIONS.X / 2, ARM_CHASSIS_DIMENSIONS.Y / 2 - 0.004f, 0.0f);
for (int i = 0; i < Globals.Instance.multicopterPropellerCount[multicopterType]; i++)
{
BoxShapeProperties chassisShapeProperties = new BoxShapeProperties(
MULTICOPTER_ARM_MASS,
new Pose(MathHelper.RotateAroundAxis_Y(multicopterCenter, new Vector3(), angleOffset * i),
Quaternion.FromAxisAngle(0.0f, -1.0f, 0.0f, angleOffset * i)),
ARM_CHASSIS_DIMENSIONS
);
// Set the material properties
// -> Restitution = how bouncy the material is
// -> Static friction = Friction between non-moving surfaces
// -> Dynamic friction = Friction betwwen moving surfaces
chassisShapeProperties.Material = new MaterialProperties("high friction", 0.1f, 1.0f, 20.0f);
chassisShapeProperties.Name = State.Name + " Arm chassis " + i;
// Add the chassis descriptions
State.PhysicsPrimitives.Add(new BoxShape(chassisShapeProperties));
}
// Gesamtmasse des Multicopters im Statistics Fenster anzeigen
WindowStatistics.OnSingleDisplayUpdate onSingleDisplayUpdate = new WindowStatistics.OnSingleDisplayUpdate();
onSingleDisplayUpdate.multicopterWeight = Globals.Instance.multicopterPropellerCount[multicopterType] * MULTICOPTER_ARM_MASS;
windowStatisticsEventsPort.Post(onSingleDisplayUpdate);
// Create and insert the physics entity
base.CreateAndInsertPhysicsEntity(physicsEngine);
// Increase physics fidelity
base.PhysicsEntity.SolverIterationCount = 60;
}
void AddDomino(Vector3 startPosition, int pieces)
{
Vector3 dim = new Vector3(0.5f, 1.0f, 0.15f);
float spacing = 0.45f;
for (int i = 0; i < pieces; i++)
{
Vector3 pos = new Vector3(0,
0.02f, i * spacing) + startPosition;
BoxShapeProperties shape = null;
shape = new BoxShapeProperties(10f, new Pose(), dim);
shape.Material = new MaterialProperties("domino", 0.2f, 0.5f, 0.5f);
SingleShapeEntity entity = new SingleShapeEntity(new BoxShape(shape), pos);
entity.State.Name = "Domino Box:" + i.ToString();
SimulationEngine.GlobalInstancePort.Insert(entity);
}
}
// 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++;
}
//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++;
}
/// <summary>
/// Custom constructor for building model from hardcoded values. Used to create entity programmatically
/// </summary>
/// <param name="initialPos"></param>
public ScribblerRobot(Vector3 initialPos)
{
MASS = 0.68f; //kg (1.5 lbs)
// the default settings approximate the Scribbler chassis
CHASSIS_DIMENSIONS = new Vector3(0.1524f, //meters wide (6 in)
0.0508f, //meters high (2 in)
0.1905f); //meters long (7.5 in)
FRONT_WHEEL_MASS = 0.01f;
CHASSIS_CLEARANCE = 0.006f; //0.25 in
FRONT_WHEEL_RADIUS = 0.0381f; //1.5 in
CASTER_WHEEL_RADIUS = 0.0127f; //0.5 in
FRONT_WHEEL_WIDTH = 0.006f; //0.25 in
//CASTER_WHEEL_WIDTH = 0.008f; //not currently used
FRONT_AXLE_DEPTH_OFFSET = 0; // distance of the axle from the center of robot //3.75 in
base.State.Name = "ScribblerRobot";
base.State.MassDensity.Mass = MASS;
base.State.Pose.Position = initialPos;
//this is because the robot is hevily back weighted
base.State.MassDensity.CenterOfMass = new Pose(new Vector3(-1f, -1f, -1f));
// reference point for all shapes is the projection of
// the center of mass onto the ground plane
// (basically the spot under the center of mass, at Y = 0, or ground level)
// NOTE: right/left is from the perspective of the robot, looking forward
// NOTE: X = width of robot (right to left), Y = height, Z = length
// chassis position
BoxShapeProperties motorBaseDesc = new BoxShapeProperties("Scribbler Body", MASS,
new Pose(new Vector3(
0, // Chassis center is also the robot center, so use zero for the X axis offset
CHASSIS_CLEARANCE + CHASSIS_DIMENSIONS.Y / 2, // chassis is off the ground and its center is DIM.Y/2 above the clearance
0)), // any offset in the z/length axis
CHASSIS_DIMENSIONS);
motorBaseDesc.Material = new MaterialProperties("high friction", 0.0f, 1.0f, 20.0f);
motorBaseDesc.Name = "Chassis";
ChassisShape = new BoxShape(motorBaseDesc);
// rear wheel is also called the caster
CASTER_WHEEL_POSITION = new Vector3(0, // center of chassis widthwise
CASTER_WHEEL_RADIUS, // distance from ground
CHASSIS_DIMENSIONS.Z / 2 - CASTER_WHEEL_RADIUS / 2); // all the way at the back of the robot
RIGHT_FRONT_WHEEL_POSITION = new Vector3(
+CHASSIS_DIMENSIONS.X / 2 - FRONT_WHEEL_WIDTH / 2,// left of center
FRONT_WHEEL_RADIUS,// distance from ground of axle
FRONT_AXLE_DEPTH_OFFSET); // distance from center, on the z-axis
LEFT_FRONT_WHEEL_POSITION = new Vector3(
-CHASSIS_DIMENSIONS.X / 2 - FRONT_WHEEL_WIDTH / 2,// right of center
FRONT_WHEEL_RADIUS,// distance from ground of axle
FRONT_AXLE_DEPTH_OFFSET); // distance from center, on the z-axis
MotorTorqueScaling = 10;
}
/// <summary>
/// Initialization constructor
/// </summary>
/// <param name="initialPos"></param>
public Sample4x4Vehicle(Vector3 initialPos)
{
Mass = 1200;
ChassisDimensions = new Vector3(1.61f, 0.58f, 2.89f);
ChassisClearance = 0.43f;
FrontWheelRadius = 0.35f;
RearWheelRadius = 0.35f;
FrontWheelWidth = 0.17f; // not used?
RearWheelWidth = 0.17f; // not used?
FrontAxleDepthOffset = -1.08f; // distance of the axle from the center of robot
RearAxleDepthOffset = 0.83f; // distance of the axle from the center of the robot
WheelBase = RearAxleDepthOffset - FrontAxleDepthOffset;
DistanceBetweenWheels = ChassisDimensions.X;
SuspensionTravel = 0.2f;
FrontWheelMesh = "4x4Wheel.obj";
RearWheelMesh = "4x4Wheel.obj";
State.Assets.Mesh = "4x4Body.obj";
base.State.Name = "MotorBaseWithFourWheels";
base.State.MassDensity.Mass = Mass;
base.State.Pose.Position = initialPos;
// reference point for all shapes is the projection of
// the center of mass onto the ground plane
// (basically the spot under the center of mass, at Y = 0, or ground level)
// chassis position
BoxShapeProperties chassis = new BoxShapeProperties(
"chassis",
Mass * 5 / 6,
new Pose(
new Vector3(
0,
ChassisClearance + ChassisDimensions.Y / 2,
0
)
),
ChassisDimensions
);
chassis.Material = new MaterialProperties("high friction", 0.0f, 1.0f, 20.0f);
float proportion = 2f / 3f;
BoxShapeProperties cabin = new BoxShapeProperties(
"cabin",
Mass / 6,
new Pose(
new Vector3(
0,
ChassisClearance + 3 * ChassisDimensions.Y / 2,
ChassisDimensions.Z * (1 - proportion) / 2
)
),
new Vector3(
ChassisDimensions.X,
ChassisDimensions.Y,
ChassisDimensions.Z * proportion
)
);
cabin.Material = chassis.Material;
ChassisShape = new List <BoxShape>();
ChassisShape.Add(new BoxShape(chassis));
ChassisShape.Add(new BoxShape(cabin));
// NOTE: right/left is from the perspective of the robot, looking forward
FrontWheelMass = 10;
RearWheelMass = 10;
RightFrontWheelPosition = new Vector3(
(ChassisDimensions.X + 0.001f) / 2, // left of center
FrontWheelRadius, // distance from ground of axle
FrontAxleDepthOffset
); // distance from center, on the z-axis
LeftFrontWheelPosition = new Vector3(
-(ChassisDimensions.X + 0.001f) / 2, // left of center
FrontWheelRadius, // distance from ground of axle
FrontAxleDepthOffset
); // distance from center, on the z-axis
RightRearWheelPosition = new Vector3(
(ChassisDimensions.X + 0.001f) / 2, // left of center
RearWheelRadius, // distance from ground of axle
RearAxleDepthOffset
); // distance from center, on the z-axis
LeftRearWheelPosition = new Vector3(
-(ChassisDimensions.X + 0.001f) / 2, // left of center
RearWheelRadius, // distance from ground of axle
RearAxleDepthOffset
); // distance from center, on the z-axis
MotorTorqueScaling = 20;
}
/// <summary>
/// Initialization constructor
/// </summary>
/// <param name="initialPos"></param>
public SimpleFourByFour(Vector3 initialPos)
{
Mass = 400;
// the default settings are pulled out of thin air
ChassisDimensions = new Vector3(1.5f, 0.5f, 2f);
ChassisClearance = 0.2f;
FrontWheelRadius = 0.3f;
RearWheelRadius = 0.3f;
FrontWheelWidth = 0.3f; // not used?
RearWheelWidth = 0.3f; // not used?
FrontAxleDepthOffset = -0.7f; // distance of the axle from the center of robot
RearAxleDepthOffset = 0.7f; // distance of the axle from the center of the robot
WheelBase = RearAxleDepthOffset - FrontAxleDepthOffset;
DistanceBetweenWheels = ChassisDimensions.X;
base.State.Name = "MotorBaseWithFourWheels";
base.State.MassDensity.Mass = Mass;
base.State.Pose.Position = initialPos;
// reference point for all shapes is the projection of
// the center of mass onto the ground plane
// (basically the spot under the center of mass, at Y = 0, or ground level)
// chassis position
BoxShapeProperties motorBaseDesc = new BoxShapeProperties(
"chassis",
Mass,
new Pose(
new Vector3(
0, // Chassis center is also the robot center, so use zero for the X axis offset
ChassisClearance + ChassisDimensions.Y / 2, // chassis is off the ground and its center is DIM.Y/2 above the clearance
0)
), // no offset in the z/depth axis, since again, its center is the robot center
ChassisDimensions);
motorBaseDesc.Material = new MaterialProperties("high friction", 0.0f, 1.0f, 20.0f);
motorBaseDesc.Name = "Chassis";
ChassisShape = new List <BoxShape>();
ChassisShape.Add(new BoxShape(motorBaseDesc));
// NOTE: right/left is from the perspective of the robot, looking forward
FrontWheelMass = 10;
RearWheelMass = 10;
RightFrontWheelPosition = new Vector3(
(ChassisDimensions.X + FrontWheelWidth) / 2, // left of center
FrontWheelRadius, // distance from ground of axle
FrontAxleDepthOffset
); // distance from center, on the z-axis
LeftFrontWheelPosition = new Vector3(
-(ChassisDimensions.X + FrontWheelWidth) / 2, // left of center
FrontWheelRadius, // distance from ground of axle
FrontAxleDepthOffset
); // distance from center, on the z-axis
RightRearWheelPosition = new Vector3(
(ChassisDimensions.X + RearWheelWidth) / 2, // left of center
RearWheelRadius, // distance from ground of axle
RearAxleDepthOffset
); // distance from center, on the z-axis
LeftRearWheelPosition = new Vector3(
-(ChassisDimensions.X + RearWheelWidth) / 2, // left of center
RearWheelRadius, // distance from ground of axle
RearAxleDepthOffset
); // distance from center, on the z-axis
MotorTorqueScaling = 20;
}
public TTPioneer3DX() { } // TT - Flow-on effect of name change
/// <summary>
/// Initialization constructor
/// </summary>
/// <param name="initialPos"></param>
public TTPioneer3DX(Vector3 initialPos)
{
MASS = 9;
// the default settings are approximating a Pioneer 3-DX activMedia robot
CHASSIS_DIMENSIONS = new Vector3(0.393f, 0.18f, 0.40f);
CHASSIS_CLEARANCE = 0.05f;
FRONT_WHEEL_RADIUS = 0.08f;
CASTER_WHEEL_RADIUS = 0.025f; // = CHASSIS_CLEARANCE / 2; // to keep things simple we make caster a bit bigger
FRONT_WHEEL_WIDTH = 4.74f; //not used
CASTER_WHEEL_WIDTH = 0.02f; //not used
FRONT_AXLE_DEPTH_OFFSET = -0.05f; // distance of the axle from the center of robot
base.State.Name = "MotorBaseWithThreeWheels";
base.State.MassDensity.Mass = MASS;
base.State.Pose.Position = initialPos;
// reference point for all shapes is the projection of
// the center of mass onto the ground plane
// (basically the spot under the center of mass, at Y = 0, or ground level)
// chassis position
BoxShapeProperties motorBaseDesc = new BoxShapeProperties("chassis", MASS,
new Pose(new Vector3(
0, // Chassis center is also the robot center, so use zero for the X axis offset
CHASSIS_CLEARANCE + CHASSIS_DIMENSIONS.Y / 2, // chassis is off the ground and its center is DIM.Y/2 above the clearance
0)), // no offset in the z/depth axis, since again, its center is the robot center
CHASSIS_DIMENSIONS);
motorBaseDesc.Material = new MaterialProperties("high friction", 0.0f, 1.0f, 20.0f);
motorBaseDesc.Name = "Chassis";
ChassisShape = new BoxShape(motorBaseDesc);
// rear wheel is also called the caster
CASTER_WHEEL_POSITION = new Vector3(0, // center of chassis
CASTER_WHEEL_RADIUS, // distance from ground
CHASSIS_DIMENSIONS.Z / 2 - CASTER_WHEEL_RADIUS); // all the way at the back of the robot
// NOTE: right/left is from the perspective of the robot, looking forward
FRONT_WHEEL_MASS = 0.10f;
RIGHT_FRONT_WHEEL_POSITION = new Vector3(
CHASSIS_DIMENSIONS.X / 2 + 0.01f,// left of center
FRONT_WHEEL_RADIUS,// distance from ground of axle
FRONT_AXLE_DEPTH_OFFSET); // distance from center, on the z-axis
LEFT_FRONT_WHEEL_POSITION = new Vector3(
-CHASSIS_DIMENSIONS.X / 2 - 0.01f,// right of center
FRONT_WHEEL_RADIUS,// distance from ground of axle
FRONT_AXLE_DEPTH_OFFSET); // distance from center, on the z-axis
MotorTorqueScaling = 20;
}
/// <summary>
/// Custom constructor for building model from hardcoded values. Used to create entity programmatically
/// </summary>
/// <param name="initialPos"></param>
public TTLegoNXTTribot(Vector3 initialPos)
{
MASS = 0.5f; //kg
// the default settings approximate the Lego NXT baseline chassis
CHASSIS_DIMENSIONS = new Vector3(0.105f, //meters wide
0.12f, //meters high
0.14f); //meters long
FRONT_WHEEL_MASS = 0.01f;
CHASSIS_CLEARANCE = 0.015f;
FRONT_WHEEL_RADIUS = 0.025f;
CASTER_WHEEL_RADIUS = 0.0125f;
FRONT_WHEEL_WIDTH = 0.028f;
CASTER_WHEEL_WIDTH = 0.008f; //not currently used, but dim is accurate
FRONT_AXLE_DEPTH_OFFSET = -0.005f; // distance of the axle from the center of robot
base.State.Name = "LegoNXTTribot";
base.State.MassDensity.Mass = MASS;
base.State.Pose.Position = initialPos;
// reference point for all shapes is the projection of
// the center of mass onto the ground plane
// (basically the spot under the center of mass, at Y = 0, or ground level)
// NOTE: right/left is from the perspective of the robot, looking forward
// NOTE: X = width of robot (right to left), Y = height, Z = length
// chassis position
BoxShapeProperties motorBaseDesc = new BoxShapeProperties("NXT brick", MASS,
new Pose(new Vector3(
0, // Chassis center is also the robot center, so use zero for the X axis offset
CHASSIS_CLEARANCE + CHASSIS_DIMENSIONS.Y / 2, // chassis is off the ground and its center is DIM.Y/2 above the clearance
0.025f)), // minor offset in the z/depth axis
CHASSIS_DIMENSIONS);
motorBaseDesc.Material = new MaterialProperties("high friction", 0.0f, 1.0f, 20.0f);
motorBaseDesc.Name = "Chassis";
ChassisShape = new BoxShape(motorBaseDesc);
// rear wheel is also called the caster
CASTER_WHEEL_POSITION = new Vector3(0, // center of chassis
CASTER_WHEEL_RADIUS, // distance from ground
CHASSIS_DIMENSIONS.Z / 2 + 0.055f); // all the way at the back of the robot
// Deleted the extra offset of +- FRONT_WHEEL_WIDTH / 2
RIGHT_FRONT_WHEEL_POSITION = new Vector3(
+CHASSIS_DIMENSIONS.X / 2,// left of center
FRONT_WHEEL_RADIUS,// distance from ground of axle
FRONT_AXLE_DEPTH_OFFSET); // distance from center, on the z-axis
LEFT_FRONT_WHEEL_POSITION = new Vector3(
-CHASSIS_DIMENSIONS.X / 2,// right of center
FRONT_WHEEL_RADIUS,// distance from ground of axle
FRONT_AXLE_DEPTH_OFFSET); // distance from center, on the z-axis
MotorTorqueScaling = 30;
}
private void AddCube(Vector3 position)
{
BoxShapeProperties cBoxShape = null;
SingleShapeEntity cBoxEntity = null;
cBoxShape = new BoxShapeProperties(2.0f, new Pose(), new Vector3(0.5f, 0.5f, 0.5f));
cBoxEntity = new SingleShapeEntity(new BoxShape(cBoxShape), position);
cBoxEntity.State.Name = "cube:" + Guid.NewGuid().ToString();
SimulationEngine.GlobalInstancePort.Insert(cBoxEntity);
}
private void AddArena()
{
BoxShapeProperties tBoxShape = null;
SingleShapeEntity tBoxEntity = null;
tBoxShape = new BoxShapeProperties(500f, new Pose(), new Vector3(5f, 0.25f, 0.25f));
tBoxEntity = new SingleShapeEntity(new BoxShape(tBoxShape), new Vector3(0f, 0.5f, 2.63f));
tBoxEntity.State.Name = "arena top";
SimulationEngine.GlobalInstancePort.Insert(tBoxEntity);
BoxShapeProperties lBoxShape = null;
SingleShapeEntity lBoxEntity = null;
lBoxShape = new BoxShapeProperties(500f, new Pose(), new Vector3(0.25f, 0.25f, 5f));
lBoxEntity = new SingleShapeEntity(new BoxShape(lBoxShape), new Vector3(-2.63f, 0.5f, 0f));
lBoxEntity.State.Name = "arena left";
SimulationEngine.GlobalInstancePort.Insert(lBoxEntity);
BoxShapeProperties rBoxShape = null;
SingleShapeEntity rBoxEntity = null;
rBoxShape = new BoxShapeProperties(500f, new Pose(), new Vector3(0.25f, 0.25f, 5f));
rBoxEntity = new SingleShapeEntity(new BoxShape(rBoxShape), new Vector3(2.63f, 0.5f, 0f));
rBoxEntity.State.Name = "arena right";
SimulationEngine.GlobalInstancePort.Insert(rBoxEntity);
BoxShapeProperties bBoxShape = null;
SingleShapeEntity bBoxEntity = null;
bBoxShape = new BoxShapeProperties(500f, new Pose(), new Vector3(5f, 0.25f, 0.25f));
bBoxEntity = new SingleShapeEntity(new BoxShape(bBoxShape), new Vector3(0f, 0.5f, -2.63f));
bBoxEntity.State.Name = "arena bottom";
SimulationEngine.GlobalInstancePort.Insert(bBoxEntity);
}
/// <summary>
/// Initialization constructor
/// </summary>
/// <param name="localPose"></param>
public SonarEntity(Pose localPose)
{
// create a new instance of the sonar pose so we dont re-use the raycast reference
// That reference will be updated regularly
BoxShapeProperties box = new BoxShapeProperties("Sonar", 0.5f,
localPose,
new Vector3(0.1f, 0.1f, 0.1f));
_sonarBox = new BoxShape(box);
State.Assets.Effect = "LaserRangeFinder.fx";
}
public void AddObstacle(Vector3 dimension_passed, Vector3 position_passed, string name)
{
Vector3 position = new Vector3(position_passed.X, position_passed.Z, -(position_passed.Y));
Vector3 dimension = new Vector3(dimension_passed.X, dimension_passed.Z, -(dimension_passed.Y));
BoxShapeProperties tBoxShape = new BoxShapeProperties(10000f, new Pose(), dimension);
tBoxShape.Material = new MaterialProperties("tbox", 0.4f, 0.4f, 0.6f);
tBoxShape.DiffuseColor = new Vector4(0.5f, 0.5f, 0.5f, 1.0f);
SingleShapeEntity Obstacle = new SingleShapeEntity(new BoxShape(tBoxShape), position); ;
Obstacle.State.Assets.DefaultTexture = "env2.bmp";
Obstacle.State.Name = name;
SimulationEngine.GlobalInstancePort.Insert(Obstacle);
this.BoundaryList.Add(Obstacle);
}
