diff --git a/01_Code/physical_computing_interface/assembly/main.js b/01_Code/physical_computing_interface/assembly/main.js
index c3cb715c2caf99eb99ae9c5e78a4ec3f8de0a5a4..3ba2c18b5b2f5b5d387f12b75763602b9dc7d67c 100644
--- a/01_Code/physical_computing_interface/assembly/main.js
+++ b/01_Code/physical_computing_interface/assembly/main.js
@@ -15,7 +15,7 @@ var RAD_TO_DEG = 180 / Math.PI;
///////////////////////////voxels///////////////////////////////
var voxel;
-var voxelSpacing=3.0;
+var voxelSpacing=1.0;
var voxelLocations=[];
var voxelSlices=[];
var voxelSlicesCount=[];
@@ -36,7 +36,7 @@ var grid=[];
///////////////////////////robot///////////////////////////////
var numberOfRobots=1; //change number of robots
-var speed=200; //change to speed up the simulation
+var speed=20; //change to speed up the simulation
var THREE1=[];
var robotBones = [];
var joints = [];
@@ -75,6 +75,8 @@ var path= [];
var totalNumberofSteps=[];
+var locations=[]
+
///////////////////////function calls///////////////////////////////
declareGlobals();
@@ -87,8 +89,8 @@ for( i=0;i<numberOfRobots;i++)
setupGUI(i);
THREERobotCall(i);
defaultRobot(i);
-
- // startMovement(i,false);
+ buildHelperMeshes(i);
+ assemble(i);
}
@@ -121,7 +123,7 @@ function declareGlobals(){
leg.push(1);
voxelNormal.push(180);
normalAdjustmentVector.push(new THREE.Vector3( 0, 0, 0));
- startLocations.push(new THREE.Vector3(0,0,0) );
+ startLocations.push(new THREE.Vector3(0*voxelSpacing,10*voxelSpacing,0) );
THREE1dir.push(new THREE.Vector3(1,0,0) );
THREE1Pos.push(new THREE.Vector3(0,0,0) );
@@ -142,8 +144,8 @@ function declareGlobals(){
number:20,
delay:1000/speed,
timeout:0,
- cHeight:1.4*voxelSpacing,
- showPath:false,
+ cHeight:5.0*voxelSpacing,
+ showPath:true,
normals:[],
changeLegs:[],
changeRotation:[],
@@ -214,7 +216,7 @@ function init() {
window.addEventListener('resize', onWindowResize, false);
- var size = 10;
+ var size = 20;
var step = 20;
var gridHelper = new THREE.GridHelper(size, step);
@@ -307,6 +309,31 @@ function THREERobotCall(robotIndex) {
joints[robotIndex].push(joint);
switch (jointNumber) {
+ // case 0:
+ // joint.rotation.x = Math.PI / 2
+ // break
+ // case 1:
+ // joint.rotation.z = Math.PI / 2 //changed
+ // // joint.rotation.y = Math.PI / 2 //changed
+ // break
+ // case 2:
+ // joint.rotation.z = Math.PI / 2 //changed
+ // // joint.rotation.y = Math.PI / 2 //changed
+ // // joint.rotation.x = Math.PI / 2
+ // break
+ // case 3:
+ // joint.rotation.z = Math.PI / 2 //changed
+ // // joint.rotation.y = Math.PI
+ // break
+ // case 4:
+ // joint.rotation.x = Math.PI / 2 //changed
+ // // joint.rotation.x = Math.PI / 2
+ // // joint.rotation.y = Math.PI / 2
+ // break
+ // case 5:
+ // joint.rotation.x = Math.PI / 2
+ // group.rotation.y = Math.PI / 2
+ // break;
case 0:
joint.rotation.x = Math.PI / 2
break
@@ -354,7 +381,7 @@ function THREERobotCall(robotIndex) {
{
carriedVoxel[robotIndex]=voxel.clone();
- carriedVoxel[robotIndex].position.x=x+voxelSpacing;
+ carriedVoxel[robotIndex].position.x=x+2.5*voxelSpacing;
carriedVoxel[robotIndex].position.y=y;
carriedVoxel[robotIndex].position.z=z ;
linkGeo.add( carriedVoxel[robotIndex] );
@@ -376,142 +403,20 @@ function THREERobotCall(robotIndex) {
// robotBones[robotIndex][3].rotation.y = angles[robotIndex][3];
// robotBones[robotIndex][4].rotation.z = angles[robotIndex][4];
// robotBones[robotIndex][5].rotation.y = angles[robotIndex][5];
+
robotBones[robotIndex][0].rotation.z = angles[robotIndex][0];
robotBones[robotIndex][1].rotation.y = angles[robotIndex][1];
robotBones[robotIndex][2].rotation.y = angles[robotIndex][2];
robotBones[robotIndex][3].rotation.x = angles[robotIndex][3];
robotBones[robotIndex][4].rotation.y = angles[robotIndex][4];
robotBones[robotIndex][5].rotation.z = angles[robotIndex][5];
- },
-
- setAngle(robotIndex,index, angle) {
- angles[robotIndex][index] = angle;
- this.setAngles(robotIndex,angles[robotIndex].slice());
- },
- };
-}
-
-function THREERobotCallOld(robotIndex) {
- THREERobot= function (robotIndex,V_initial, limits, scene) {
-
- THREE1[robotIndex] = new THREE.Group();
-
- var colors = [
- 0xaaaaaa,
- 0xbbbbbb,
- 0xbcbcbc,
- 0xcbcbcb,
- 0xcccccc,
- 0x0,
- ];
-
- let parentObject = THREE1[robotIndex];
- robotBones[robotIndex] = [];
- joints[robotIndex] = [];
-
- function createCube(x, y, z, w, h, d, min, max, jointNumber) {
- var thicken = 1;
-
- var w_thickened = Math.abs(w) + thicken;
- var h_thickened = Math.abs(h) + thicken;
- var d_thickened = Math.abs(d) + thicken;
-
- var material = new THREE.MeshLambertMaterial({ color: colors[jointNumber],});
- var geometry = new THREE.CubeGeometry(w_thickened, h_thickened, d_thickened);
- var mesh = new THREE.Mesh(geometry, material);
-
-
- mesh.position.set(w / 2, h / 2, d / 2);
- var group = new THREE.Object3D();
- group.position.set(x, y, z);
- group.add(mesh);
-
- // min = min / 180 * Math.PI
- // max = max / 180 * Math.PI
-
- var jointGeo1 = new THREE.CylinderGeometry(0.8, 0.8, 0.8 * 2, 32, 32, false, -min, 2 * Math.PI - max + min)
- var jointGeoMax = new THREE.CylinderGeometry(0.8, 0.8, 0.8 * 2, 32, 32, false, -max, max)
- var jointGeoMin = new THREE.CylinderGeometry(0.8, 0.8, 0.8 * 2, 32, 32, false, 0, -min)
- var jointMesh1 = new THREE.Mesh(jointGeo1, new THREE.MeshBasicMaterial({
- color: 0x000000,//0xffbb00,
- }));
- var jointMeshMax = new THREE.Mesh(jointGeoMax, new THREE.MeshBasicMaterial({
- color: 0x000000,//0x009900,
- }));
- var jointMeshMin = new THREE.Mesh(jointGeoMin, new THREE.MeshBasicMaterial({
- color: 0x000000,//0xdd2200,
- }));
-
- var joint = new THREE.Group();
- joint.add(jointMeshMax, jointMeshMin, jointMesh1);
-
- joints[robotIndex].push(joint);
-
- switch (jointNumber) {
- case 0:
- joint.rotation.x = Math.PI / 2;
- break;
- case 1:
- // joint.rotation.x = Math.PI / 2
- break;
- case 2:
- // joint.rotation.x = Math.PI / 2
- break;
- case 3:
- // joint.rotation.z = Math.PI / 2;
- break;
- case 4:
- joint.rotation.x = Math.PI / 2;
- break;
- }
-
- group.add(joint);
- return group;
- }
-
- let x = 0,
- y = 0,
- z = 0;
-
- // V_initial.push([0, 0, 0]) // add a 6th pseudo link for 6 axis
-
- for (let i = 0; i < V_initial.length; i++) {
- var link = V_initial[i];
-
-
-
- var linkGeo = createCube(x, y, z, link[0], link[1], link[2], limits[i][0], limits[i][1], i);
- x = link[0];
- y = link[1];
- z = link[2];
-
- if(i==5)
- {
-
- carriedVoxel[robotIndex]=voxel.clone();
- carriedVoxel[robotIndex].position.x=x-voxelSpacing/2-0.5;
- carriedVoxel[robotIndex].position.y=y;
- carriedVoxel[robotIndex].position.z=z-2;
- linkGeo.add( carriedVoxel[robotIndex] );
- }
-
- parentObject.add(linkGeo);
- parentObject = linkGeo;
- robotBones[robotIndex].push(linkGeo);
- }
-
- scene.add(THREE1[robotIndex]);
- };
-
- THREERobot.prototype = {
- setAngles(robotIndex,angles1) {
- robotBones[robotIndex][0].rotation.z = angles[robotIndex][0];
- robotBones[robotIndex][1].rotation.y = angles[robotIndex][1];
- robotBones[robotIndex][2].rotation.y = angles[robotIndex][2];
- robotBones[robotIndex][3].rotation.y = angles[robotIndex][3];
- robotBones[robotIndex][4].rotation.z = angles[robotIndex][4];
- // robotBones[robotIndex][5].rotation.z = angles[robotIndex][5];
+ // robotBones[robotIndex][0].rotation.z =angles[robotIndex][0];
+ // robotBones[robotIndex][1].rotation.x =angles[robotIndex][1];
+ // robotBones[robotIndex][2].rotation.x =angles[robotIndex][2];
+ // robotBones[robotIndex][3].rotation.x =angles[robotIndex][3];
+ // robotBones[robotIndex][4].rotation.z =angles[robotIndex][4];
+ // robotBones[robotIndex][5].rotation.z =angles[robotIndex][5];
},
setAngle(robotIndex,index, angle) {
@@ -546,19 +451,26 @@ function defaultRobot(robotIndex) {
wrist_2_length =scale* d5;
wrist_3_length =scale* d6;
geo = [
- [scale*200, 0, 0],
- [0, 0, scale*612.7],
+ // [scale*200, 0, scale*50],
+ // [-scale*100, 0, scale*600],
+ // [0, 0, scale*600],
+ // [scale*200, 0,0],
+ // [0, 0, scale*200],
+ // [scale*200, 0, 0],
+ [scale*200, 0, scale*100],
+ [0, 0, scale*800],
[0, 0,scale*200],
- [scale*1200.7, 0, 0],
- [scale*100, 0, 0],
- [scale*100, 0, 0],
+ [scale*800, 0, 0],
+ [scale*200, 0, 0],
+ [scale*200, 0, 0],
+
];
defaultRobotState[robotIndex] = {
target: {
position: {
- x: 10,
- y: 0,
- z: 10,
+ x: startLocations[robotIndex].x,
+ y: startLocations[robotIndex].y,
+ z: startLocations[robotIndex].z,
},
rotation: {
x: Math.PI,
@@ -643,6 +555,8 @@ function defaultRobot(robotIndex) {
VisualRobot[robotIndex].setAngle(robotIndex,1,restAngle*RAD_TO_DEG);
VisualRobot[robotIndex].setAngle(robotIndex,2,restAngle*RAD_TO_DEG);
VisualRobot[robotIndex].setAngle(robotIndex,3,restAngle*RAD_TO_DEG);
+ VisualRobot[robotIndex].setAngle(robotIndex,4,restAngle*RAD_TO_DEG);
+ VisualRobot[robotIndex].setAngle(robotIndex,5,restAngle*RAD_TO_DEG);
// var target= new THREE.Vector3(guiControls[robotIndex].x,guiControls[robotIndex].y,guiControls[robotIndex].z);
// updateGUI();
@@ -650,113 +564,7 @@ function defaultRobot(robotIndex) {
targetControl(robotIndex);
THREE1[robotIndex].position.x=0;
THREE1[robotIndex].position.y=0;
- THREE1[robotIndex].position.z=voxelSpacing/2;
- updateAngles(robotIndex);
-
-}
-
-function defaultRobotOld(robotIndex) {
- localState = {
- jointOutOfBound: [false, false, false, false, false, false],
- };
- var maxAngleVelocity = 90.0 / (180.0 * Math.PI) / 1000.0;
- geo = [
- [0, 0, guiControls[robotIndex].offset],
- [0, 0, guiControls[robotIndex].leg1],
- [0, 0, guiControls[robotIndex].leg2],
- [0, 0, guiControls[robotIndex].offset],
- [0, 0, 0],
- [0, 0, 0],
- ];
- defaultRobotState[robotIndex] = {
- target: {
- position: {
- x: 10,
- y: 10,
- z: 10,
- },
- rotation: {
- x: Math.PI,
- y: 0,
- z: 0,
- },
- },
- angles: {
- A0: 0,
- A1: 0,
- A2: 0,
- A3: 0,
- A4: 0,
- A5: 0,
- },
- jointOutOfBound: [false, false, false, false, false, false],
- maxAngleVelocities: {
- J0: maxAngleVelocity,
- J1: maxAngleVelocity,
- J2: maxAngleVelocity,
- J3: maxAngleVelocity,
- J4: maxAngleVelocity,
- J5: maxAngleVelocity,
- },
- jointLimits: {
- J0: [-190 / 180 * Math.PI, 190 / 180 * Math.PI],
- J1: [-58 / 180 * Math.PI, 90 / 180 * Math.PI],
- J2: [-135 / 180 * Math.PI, 40 / 180 * Math.PI],
- J3: [-90 / 180 * Math.PI, 75 / 180 * Math.PI],
- J4: [-139 / 180 * Math.PI, 20 / 180 * Math.PI],
- J5: [-188 / 180 * Math.PI, 181 / 180 * Math.PI],
- },
- geometry: {
- V0: {
- x: geo[0][0],
- y: geo[0][1],
- z: geo[0][2],
- },
- V1: {
- x: geo[1][0],
- y: geo[1][1],
- z: geo[1][2],
- },
- V2: {
- x: geo[2][0],
- y: geo[2][1],
- z: geo[2][2],
- },
- V3: {
- x: geo[3][0],
- y: geo[3][1],
- z: geo[3][2],
- },
- V4: {
- x: geo[4][0],
- y: geo[4][1],
- z: geo[4][2],
- },
- },
- };
-
- THREESimulationRobot[robotIndex] = new THREE.Group();
- scene.add(THREESimulationRobot[robotIndex]);
-
- var geometry = Object.values(defaultRobotState[robotIndex].geometry).map((val, i, array) => [val.x, val.y, val.z]);
- var jointLimits = Object.values(defaultRobotState[robotIndex].jointLimits);
-
- VisualRobot[robotIndex] = new THREERobot(robotIndex,geometry, jointLimits, THREESimulationRobot[robotIndex]);
- // var angles = Object.values(defaultRobotState.angles);
- // VisualRobot[robotIndex].setAngles(angles);
- var restAngle=60;
- // VisualRobot[robotIndex].setAngle(0,restAngle/ 180 * Math.PI);
- VisualRobot[robotIndex].setAngle(robotIndex,1,restAngle*RAD_TO_DEG);
- VisualRobot[robotIndex].setAngle(robotIndex,2,restAngle*RAD_TO_DEG);
- VisualRobot[robotIndex].setAngle(robotIndex,3,restAngle*RAD_TO_DEG);
-
- // var target= new THREE.Vector3(guiControls[robotIndex].x,guiControls[robotIndex].y,guiControls[robotIndex].z);
- // updateGUI();
-
- targetControl(robotIndex);
- THREE1[robotIndex].position.x=robotState[robotIndex].leg2Pos.x*voxelSpacing;
- THREE1[robotIndex].position.y=robotState[robotIndex].leg2Pos.y*voxelSpacing;
- THREE1[robotIndex].position.z=voxelSpacing/2;
+ THREE1[robotIndex].position.z=0;
updateAngles(robotIndex);
}
@@ -813,63 +621,14 @@ function updateRobotGeometry1(robotIndex){
updateAngles(robotIndex);
}
-function updateRobotGeometryOld(robotIndex){
- geo = [
- [0, 0, guiControls[robotIndex].offset],
- [0, 0, guiControls[robotIndex].leg1],
- [0, 0, guiControls[robotIndex].leg2],
- [0, 0, guiControls[robotIndex].offset],
- [0, 0, 0],
- [0, 0, 0],
- ];
-
- defaultRobotState[robotIndex].geometry ={
- V0: {
- x: geo[0][0],
- y: geo[0][1],
- z: geo[0][2],
- },
- V1: {
- x: geo[1][0],
- y: geo[1][1],
- z: geo[1][2],
- },
- V2: {
- x: geo[2][0],
- y: geo[2][1],
- z: geo[2][2],
- },
- V3: {
- x: geo[3][0],
- y: geo[3][1],
- z: geo[3][2],
- },
- V4: {
- x: geo[4][0],
- y: geo[4][1],
- z: geo[4][2],
- },
- };
-
- var geometry = Object.values(defaultRobotState[robotIndex].geometry).map((val, i, array) => [val.x, val.y, val.z]);
- var jointLimits = Object.values(defaultRobotState[robotIndex].jointLimits);
-
- while (THREESimulationRobot[robotIndex].children.length) {
- THREESimulationRobot[robotIndex].remove(THREESimulationRobot[robotIndex].children[0]);
- }
-
- VisualRobot[robotIndex] = new THREERobot(robotIndex,geometry, jointLimits, THREESimulationRobot[robotIndex]);
-
- updateAngles(robotIndex);
-}
////////////////////////////////GUI///////////////////////////////
function setupGUI(robotIndex){
//Parameters that can be modified.
guiControls[robotIndex] = new function() {
- this.x = 10;
- this.y = 10;
- this.z = 10;
+ this.x = startLocations[robotIndex].x;
+ this.y = startLocations[robotIndex].y;
+ this.z = startLocations[robotIndex].z;
this.j1 = 0.0;
this.j2 = 0.0;
this.j3 = 0.0;
@@ -982,9 +741,9 @@ function updateAngles(robotIndex){
// const calculateAngles = (jointLimits, position, rotation, configuration) => {
const angles = []
IK[robotIndex].calculateAngles(
- defaultRobotState[robotIndex].target.position.x,
- defaultRobotState[robotIndex].target.position.y,
- defaultRobotState[robotIndex].target.position.z,
+ guiControls[robotIndex].x,
+ guiControls[robotIndex].y,
+ guiControls[robotIndex].z +2.5*voxelSpacing,
defaultRobotState[robotIndex].target.rotation.x,
defaultRobotState[robotIndex].target.rotation.y,
defaultRobotState[robotIndex].target.rotation.z,
@@ -1023,61 +782,6 @@ function updateAngles(robotIndex){
}
-function updateAnglesOld(robotIndex){
- THREE1[robotIndex].updateMatrixWorld();
- var matrix = new THREE.Matrix4();
- matrix.extractRotation( THREE1[robotIndex].matrix );
-
- //check rotation
-
- var vector = THREE1[robotIndex].position.clone().sub(target[robotIndex].position.clone().add(normalAdjustmentVector[robotIndex].clone()));
- vector.applyMatrix4(matrix);
-
- if(THREE1[robotIndex].rotation.x!=0&&THREE1[robotIndex].rotation.x!=180*DEG_TO_RAD)
- {
- vector.x = -vector.x;
- vector.add(THREE1[robotIndex].position.clone());
- }else if(THREE1[robotIndex].rotation.y!=0&&THREE1[robotIndex].rotation.y!=180*DEG_TO_RAD)
- {
- vector.y = -vector.y;
- vector.add(THREE1[robotIndex].position.clone());
-
- }else
- {
- vector = new THREE.Vector3(0,0,0).sub(vector.clone());
- vector.add(THREE1[robotIndex].position.clone());
- }
-
- var origin= new THREE.Vector3(THREE1[robotIndex].position.x, THREE1[robotIndex].position.y, vector.z);
-
- var a=ik_2d( vector.z-THREE1[robotIndex].position.z ,origin.distanceTo(vector) , guiControls[robotIndex].leg1, guiControls[robotIndex].leg2);
-
- VisualRobot[robotIndex].setAngle(robotIndex,0,Math.atan2( vector.y-THREE1[robotIndex].position.y , vector.x-THREE1[robotIndex].position.x));
- VisualRobot[robotIndex].setAngle(robotIndex,1,a.theta1);
- VisualRobot[robotIndex].setAngle(robotIndex,2,a.theta2-a.theta1);
- VisualRobot[robotIndex].setAngle(robotIndex,3,(voxelNormal[robotIndex]*DEG_TO_RAD)-(a.theta2));
- VisualRobot[robotIndex].setAngle(robotIndex,4,0);
-
- if(leg[robotIndex]==1)
- {
- guiControls[robotIndex].j1=angles[robotIndex][0]*RAD_TO_DEG;
- guiControls[robotIndex].j2=angles[robotIndex][1]*RAD_TO_DEG;
- guiControls[robotIndex].j3=angles[robotIndex][2]*RAD_TO_DEG;
- guiControls[robotIndex].j4=angles[robotIndex][3]*RAD_TO_DEG;
- guiControls[robotIndex].j5=angles[robotIndex][4]*RAD_TO_DEG;
- //later output package
- // console.log(angles[robotIndex][0]+","+angles[robotIndex][1]+","+angles[robotIndex][2]+","+angles[robotIndex][3]+","+angles[robotIndex][4]);
- }else
- {
- guiControls[robotIndex].j1=angles[robotIndex][4]*RAD_TO_DEG;
- guiControls[robotIndex].j2=angles[robotIndex][3]*RAD_TO_DEG;
- guiControls[robotIndex].j3=angles[robotIndex][2]*RAD_TO_DEG;
- guiControls[robotIndex].j4=angles[robotIndex][1]*RAD_TO_DEG;
- guiControls[robotIndex].j5=angles[robotIndex][0]*RAD_TO_DEG;
- //later output package
- // console.log(angles[robotIndex][4]+","+angles[robotIndex][3]+","+angles[robotIndex][2]+","+angles[robotIndex][1]+","+angles[robotIndex][0]);
- }
-}
function ik_2d(x, y, d1, d2) {
let dist = Math.sqrt(x ** 2 + y ** 2);
@@ -1180,6 +884,251 @@ function updateTarget(robotIndex){
target[robotIndex].position.z = guiControls[robotIndex].z;
}
+///////////////////////assembly/////////
+function assemble(robotIndex){
+ resetPath(robotIndex);
+ builtShape();
+ generatePoints(robotIndex);
+
+ // steps[robotIndex]=[startLocations[robotIndex],new THREE.Vector3(10*voxelSpacing,0*voxelSpacing,0)];
+ // showTargetPosition(robotIndex,minIndex,true);
+
+ for(var i=0;i<steps[robotIndex].length;i++){
+ if(i%2!=0){
+ moveRobot(robotIndex,true,steps[robotIndex][i]);
+ }else{
+ moveRobot(robotIndex,false,steps[robotIndex][i+1]);
+ }
+
+ }
+
+}
+
+function createPath(robotIndex,start,end){
+ var snormal=new THREE.Vector3(0,0,1);
+ var enormal=new THREE.Vector3(0,0,1);
+ var robotUp=new THREE.Vector3(0,0,1);
+ var p1=start.clone();
+ p1.add(snormal.clone().multiplyScalar(1));
+ var p2=new THREE.Vector3(0,0,0);
+ var p3=new THREE.Vector3(0,0,0);
+ var p4=end.clone();
+ p4.add(enormal.clone().multiplyScalar(1));
+
+ var nor = snormal.clone();
+ nor.add(enormal);
+ nor.normalize();
+
+ var dir=end.clone().sub(start);
+
+ var temp1=new THREE.Vector3(0,0,0);
+ var temp2=new THREE.Vector3(0,0,0);
+ nor.multiplyScalar(path[robotIndex].cHeight);
+
+
+ temp1.addVectors(start,dir.multiplyScalar(1/3));
+ temp2.addVectors(start,dir.multiplyScalar(2));
+
+
+ p2.addVectors(nor,temp1);
+ p3.addVectors(nor,temp2);
+
+ //create bezier curve
+ path[robotIndex].curve= new THREE.CubicBezierCurve3 (
+ p1,
+ p2,
+ p3,
+ p4
+ );
+ dividePath(robotIndex,snormal.clone(),enormal.clone());
+}
+
+function dividePath(robotIndex,snormal,enormal,robotUp,robotLocation){
+
+ var snormal=new THREE.Vector3(0,0,1);
+ var enormal=new THREE.Vector3(0,0,1);
+ var robotUp=new THREE.Vector3(0,0,1);
+ var robotLocation=new THREE.Vector3(0,0,1);
+
+ //points
+ var d=1/path[robotIndex].number;
+ var tempPoints=path[robotIndex].curve.getSpacedPoints(path[robotIndex].number);
+
+ var forward=new THREE.Vector3(0,0,0);
+ var tempV=tempPoints[0].clone().sub(robotLocation);
+ if(tempV.x!=0 && robotUp.x==0)
+ {
+ forward.x=tempV.x;
+ }else if(tempV.y!=0 && robotUp.y==0)
+ {
+ forward.y=tempV.y;
+ }else if(tempV.z!=0 && robotUp.z==0)
+ {
+ forward.z=tempV.z;
+ }
+ forward.normalize();
+
+ var p1=tempPoints[0];
+ var p2=tempPoints[path[robotIndex].number-1];
+ var diff=p2.clone().sub(p1);
+ diff.multiply(snormal);
+
+ //normals
+ var vnormal1=180;
+ var vnormal2=180;
+ if(!snormal.equals(enormal))
+ {
+
+ if(diff.x>0||diff.y>0||diff.z>0)
+ {
+ if(robotUp.equals(snormal))
+ {
+ vnormal1=180;
+ vnormal2=90;
+
+ }else
+ {
+ vnormal2=180;
+ vnormal1=90;
+
+ }
+
+ }else if(diff.x<0||diff.y<0||diff.z<0)
+ {
+ if(robotUp.equals(snormal))
+ {
+ vnormal1=180;
+ vnormal2=180+90;
+
+ }else
+ {
+ vnormal2=180;
+ vnormal1=180+90;
+
+ }
+ }
+ }
+ var dn=(vnormal2-vnormal1)/path[robotIndex].number;
+
+
+
+ for (var i=0;i<=path[robotIndex].number;i++)
+ {
+ path[robotIndex].normals.push(vnormal1+i*dn);
+ path[robotIndex].normalAdjustments.push(getNormalAdjustment(robotIndex,vnormal1+i*dn,robotUp,forward));
+ path[robotIndex].points.push(tempPoints[i].clone());
+
+
+ if(path[robotIndex].showPath)
+ {
+ var material = new THREE.MeshLambertMaterial({ color:0xff7171,});
+ var geometry = new THREE.SphereGeometry(0.05, 0.05, 0.05);
+ var mesh = new THREE.Mesh(geometry, material);
+ mesh.position.x=tempPoints[i].x;
+ mesh.position.y=tempPoints[i].y;
+ mesh.position.z=tempPoints[i].z;
+ scene.add(mesh);
+ }
+ }
+
+}
+
+function moveRobot(robotIndex,leaveVoxel,voxelLoc){
+ var ps= path[robotIndex].points.slice();
+ if(leaveVoxel){
+ setTimeout(function(){ buildVoxelAt(voxelLoc);showTargetPosition(robotIndex,voxelLoc,false) }, path[robotIndex].timeout);
+
+
+ }else{
+ setTimeout(function(){showTargetPosition(robotIndex,voxelLoc,true) }, path[robotIndex].timeout);
+
+ }
+ for(var i=0;i<=path[robotIndex].number;i++)
+ {
+ setTimeout(function(){ move(robotIndex); }, path[robotIndex].timeout+=path[robotIndex].delay);
+ }
+
+
+
+}
+
+function move(robotIndex){
+ guiControls[robotIndex].x=path[robotIndex].points[path[robotIndex].currentPoint].x;
+ guiControls[robotIndex].y=path[robotIndex].points[path[robotIndex].currentPoint].y;
+ guiControls[robotIndex].z=path[robotIndex].points[path[robotIndex].currentPoint].z;
+ voxelNormal[robotIndex]=path[robotIndex].normals[path[robotIndex].currentPoint];
+ normalAdjustmentVector[robotIndex]=path[robotIndex].normalAdjustments[path[robotIndex].currentPoint];
+
+ updateAngles(robotIndex);
+
+ control[robotIndex].position.x = guiControls[robotIndex].x;
+ control[robotIndex].position.y = guiControls[robotIndex].y;
+ control[robotIndex].position.z = guiControls[robotIndex].z;
+
+ target[robotIndex].position.x = guiControls[robotIndex].x;
+ target[robotIndex].position.y = guiControls[robotIndex].y;
+ target[robotIndex].position.z = guiControls[robotIndex].z;
+
+ path[robotIndex].currentPoint++;
+ if(path[robotIndex].currentPoint==path[robotIndex].points.length)
+ {
+ // if(goHome[robotIndex])
+ // {
+
+ // buildVoxelAt(robotState[robotIndex].leg1Pos.clone().multiplyScalar(voxelSpacing));
+ // //go Home
+ // // console.log("robotIndex:"+robotIndex+" s2s");
+ // startMovement(robotIndex,true);
+
+ // }else
+ // {
+ // //go to new Voxel
+ // // console.log("robotIndex:"+robotIndex+" s2s");
+ // startMovement(robotIndex,false);
+ // }
+ }
+
+}
+
+function builtShape(){
+ for(var i=0;i<gridSize;i++)
+ {
+ var t=[];
+ grid.push(t)
+ for(var j=0;j<gridSize;j++)
+ {
+ var tt=[];
+ grid[i].push(tt);
+ for(var k=0;k<gridSize;k++)
+ {
+ grid[i][j].push(false);
+ }
+ }
+ }
+ for(var i=gridSize/2.0;i<gridSize;i++)
+ {
+ grid[i][0][0]=true;
+ locations.push(new THREE.Vector3(i*voxelSpacing,0,0)); //TODO CHANGE LATER BASED ON NUMBER OF ROBOTS
+ }
+
+}
+
+function generatePoints(robotIndex){
+ for(var i=0;i<locations.length;i++){
+ steps[robotIndex].push(startLocations[robotIndex]);
+ steps[robotIndex].push(locations[i]);
+
+ }
+ console.log(steps[robotIndex])
+ for(var i=0;i<steps[robotIndex].length-1;i++){
+ createPath(robotIndex,steps[robotIndex][i],steps[robotIndex][i+1]);
+ }
+
+
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////Change Fixed End////////////////////////////
function changeEnd(robotIndex){
//TODO: send fix end and unfix end to code
@@ -1265,6 +1214,8 @@ function getRotation(dir){
////////////////////////////////Path////////////////////////////
+
+
function _createPath(robotIndex,start,end,snormal,enormal,robotUp,robotLocation){
var p1=start.clone();
p1.add(snormal.clone().multiplyScalar(1));
@@ -1386,6 +1337,8 @@ function _dividePath(robotIndex,snormal,enormal,robotUp,robotLocation){
}
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
function _BilleStep(robotIndex,stepLeg1,stepLeg2,legsNormal){
var changeLegBefore=false;
var leg1Pos=new THREE.Vector3(0,0,0);
@@ -1445,7 +1398,7 @@ function moveLeg(robotIndex){
}
-function move(robotIndex){
+function _move(robotIndex){
guiControls[robotIndex].x=path[robotIndex].points[path[robotIndex].currentPoint].x;
guiControls[robotIndex].y=path[robotIndex].points[path[robotIndex].currentPoint].y;
guiControls[robotIndex].z=path[robotIndex].points[path[robotIndex].currentPoint].z;
@@ -1517,7 +1470,7 @@ function resetPath(robotIndex){
}
//////////////////////////commands stack////////////////////////////
-function generatePoints(robotIndex){
+function _generatePoints(robotIndex){
for (var i=0; i<steps[robotIndex].length ; i++)
{
var s=_BilleStep(robotIndex,steps[robotIndex][i][0],steps[robotIndex][i][1],steps[robotIndex][i][2]);
@@ -1778,9 +1731,9 @@ function loadVoxel(){
// object.scale.y=0.04;
// object.scale.z=0.04;
// object.position.z=-1.5;
- object.scale.x=0.5;
- object.scale.y=0.5;
- object.scale.z=0.5;
+ object.scale.x=0.5/3.0;
+ object.scale.y=0.5/3.0;
+ object.scale.z=0.5/3.0;
object.position.x=-15;
object.position.y=-15;
voxel=object;
@@ -1887,18 +1840,18 @@ function buildVoxelAt(loc){
var object1=voxel.clone();
object1.position.x=loc.x;
object1.position.y=loc.y;
- object1.position.z=loc.z-voxelSpacing/2;
- if(loc.z==0){
- var color1= new THREE.Color( 0, 0, 0 )
+ object1.position.z=loc.z+voxelSpacing/2;
+ // if(loc.z==0){
+ // var color1= new THREE.Color( 0, 0, 0 )
- var material = new THREE.MeshLambertMaterial( { color: 0x000000 } );
+ // var material = new THREE.MeshLambertMaterial( { color: 0x000000 } );
- // var color = new THREE.Color( 1/globalRank, 0, 0 );
+ // // var color = new THREE.Color( 1/globalRank, 0, 0 );
- object1.material=material.clone();
- object1.material.color=color1;
+ // object1.material=material.clone();
+ // object1.material.color=color1;
- }
+ // }