From 357f33bf5471fadc5e64a27557ea5273fe28faac Mon Sep 17 00:00:00 2001
From: Chris <cmbirmingham19@gmail.com>
Date: Tue, 14 Nov 2023 12:58:14 -0800
Subject: [PATCH] Refactor Bindings
---
src/Bindings/CameraBinding.ts | 28 ++
src/Bindings/Lightbinding.ts | 58 +++
src/Bindings/MaterialBinding.ts | 251 ++++++++++++
src/Bindings/MotorBindings.ts | 80 ++++
src/Bindings/ObjectBinding.ts | 169 ++++++++
src/{ => Bindings}/RobotBinding.ts | 473 +++++------------------
src/Bindings/RobotLink.ts | 160 ++++++++
src/{ => Bindings}/SceneBinding.ts | 597 +++--------------------------
src/Bindings/WeightBinding.ts | 44 +++
src/Bindings/helpers.ts | 16 +
src/Sim.tsx | 2 +-
src/robots/demobot.ts | 3 +
12 files changed, 953 insertions(+), 928 deletions(-)
create mode 100644 src/Bindings/CameraBinding.ts
create mode 100644 src/Bindings/Lightbinding.ts
create mode 100644 src/Bindings/MaterialBinding.ts
create mode 100644 src/Bindings/MotorBindings.ts
create mode 100644 src/Bindings/ObjectBinding.ts
rename src/{ => Bindings}/RobotBinding.ts (73%)
create mode 100644 src/Bindings/RobotLink.ts
rename src/{ => Bindings}/SceneBinding.ts (63%)
create mode 100644 src/Bindings/WeightBinding.ts
create mode 100644 src/Bindings/helpers.ts
diff --git a/src/Bindings/CameraBinding.ts b/src/Bindings/CameraBinding.ts
new file mode 100644
index 00000000..f97128d8
--- /dev/null
+++ b/src/Bindings/CameraBinding.ts
@@ -0,0 +1,28 @@
+import { ArcRotateCamera, Scene as babylScene, Camera as babylCamera } from '@babylonjs/core';
+
+import Camera from "../state/State/Scene/Camera";
+import { Vector3wUnits } from "../util/unit-math";
+
+
+
+export const createArcRotateCamera = (camera: Camera.ArcRotate, bScene_: babylScene): ArcRotateCamera => {
+ const ret = new ArcRotateCamera('botcam', 0, 0, 0, Vector3wUnits.toBabylon(camera.target, 'centimeters'), bScene_);
+ ret.attachControl(bScene_.getEngine().getRenderingCanvas(), true);
+ ret.position = Vector3wUnits.toBabylon(camera.position, 'centimeters');
+ ret.panningSensibility = 100;
+ // ret.checkCollisions = true;
+ return ret;
+};
+
+export const createNoneCamera = (camera: Camera.None, bScene_: babylScene): ArcRotateCamera => {
+ const ret = new ArcRotateCamera('botcam', 10, 10, 10, Vector3wUnits.toBabylon(Vector3wUnits.zero(), 'centimeters'), bScene_);
+ ret.attachControl(bScene_.getEngine().getRenderingCanvas(), true);
+ return ret;
+};
+
+export const createCamera = (camera: Camera, bScene_: babylScene): babylCamera => {
+ switch (camera.type) {
+ case 'arc-rotate': return createArcRotateCamera(camera, bScene_);
+ case 'none': return createNoneCamera(camera, bScene_);
+ }
+};
\ No newline at end of file
diff --git a/src/Bindings/Lightbinding.ts b/src/Bindings/Lightbinding.ts
new file mode 100644
index 00000000..ab5b54e4
--- /dev/null
+++ b/src/Bindings/Lightbinding.ts
@@ -0,0 +1,58 @@
+import { ShadowGenerator, IShadowLight, PointLight, SpotLight, DirectionalLight,
+ Scene as babylScene } from '@babylonjs/core';
+
+import Node from "../state/State/Scene/Node";
+import Dict from "../util/Dict";
+import { ReferenceFramewUnits, RotationwUnits, Vector3wUnits } from "../util/unit-math";
+import { RawQuaternion, RawVector2, RawVector3 } from "../util/math";
+import LocalizedString from '../util/LocalizedString';
+import { Angle, Distance, Mass, SetOps } from "../util";
+
+
+export const createDirectionalLight = (id: string, node: Node.DirectionalLight, bScene_: babylScene, shadowGenerators_: Dict<ShadowGenerator>): DirectionalLight => {
+ const ret = new DirectionalLight(node.name[LocalizedString.EN_US], RawVector3.toBabylon(node.direction), bScene_);
+ ret.intensity = node.intensity;
+ if (node.radius !== undefined) ret.radius = node.radius;
+ if (node.range !== undefined) ret.range = node.range;
+ shadowGenerators_[id] = createShadowGenerator_(ret);
+ return ret;
+};
+
+export const createSpotLight = (id: string, node: Node.SpotLight, bScene_: babylScene, shadowGenerators_: Dict<ShadowGenerator>): SpotLight => {
+ const origin: ReferenceFramewUnits = node.origin ?? {};
+ const position: Vector3wUnits = origin.position ?? Vector3wUnits.zero();
+
+ const ret = new SpotLight(
+ node.name[LocalizedString.EN_US],
+ RawVector3.toBabylon(Vector3wUnits.toRaw(position, 'centimeters')),
+ RawVector3.toBabylon(node.direction),
+ Angle.toRadiansValue(node.angle),
+ node.exponent,
+ bScene_
+ );
+ shadowGenerators_[id] = createShadowGenerator_(ret);
+ return ret;
+};
+
+export const createPointLight = (id: string, node: Node.PointLight, bScene_: babylScene, shadowGenerators_: Dict<ShadowGenerator>): PointLight => {
+ const origin: ReferenceFramewUnits = node.origin ?? {};
+ const position: Vector3wUnits = origin.position ?? Vector3wUnits.zero();
+ const ret = new PointLight(
+ node.name[LocalizedString.EN_US],
+ RawVector3.toBabylon(Vector3wUnits.toRaw(position, 'centimeters')),
+ bScene_
+ );
+ ret.intensity = node.intensity;
+
+ shadowGenerators_[id] = createShadowGenerator_(ret);
+ ret.setEnabled(node.visible);
+ return ret;
+};
+
+const createShadowGenerator_ = (light: IShadowLight) => {
+ const ret = new ShadowGenerator(1024, light);
+ ret.useKernelBlur = false;
+ ret.blurScale = 2;
+ ret.filter = ShadowGenerator.FILTER_POISSONSAMPLING;
+ return ret;
+};
diff --git a/src/Bindings/MaterialBinding.ts b/src/Bindings/MaterialBinding.ts
new file mode 100644
index 00000000..41e1a5db
--- /dev/null
+++ b/src/Bindings/MaterialBinding.ts
@@ -0,0 +1,251 @@
+import { Texture, DynamicTexture, StandardMaterial, Color3, PBRMaterial,
+ Scene as babylScene, Material as babylMaterial, GlowLayer } from '@babylonjs/core';
+
+import Material from '../state/State/Scene/Material';
+import { Color } from '../state/State/Scene/Color';
+import Patch from "../util/Patch";
+
+
+
+export const createMaterial = (id: string, material: Material, bScene_: babylScene) => {
+ let bMaterial: babylMaterial;
+ switch (material.type) {
+ case 'basic': {
+ const basic = new StandardMaterial(id, bScene_);
+ const { color } = material;
+ if (color) {
+ switch (color.type) {
+ case 'color3': {
+ basic.diffuseColor = Color.toBabylon(color.color);
+ basic.diffuseTexture = null;
+ break;
+ }
+ case 'texture': {
+ if (!color.uri) {
+ basic.diffuseColor = new Color3(0.5, 0, 0.5);
+ } else {
+ if (id.includes('Sky')) {
+ basic.reflectionTexture = new Texture(color.uri, bScene_);
+ basic.reflectionTexture.coordinatesMode = Texture.FIXED_EQUIRECTANGULAR_MODE;
+ basic.backFaceCulling = false;
+ basic.disableLighting = true;
+ } else if (id === 'Container') {
+ const myDynamicTexture = new DynamicTexture("dynamic texture", 1000, bScene_, true);
+ // myDynamicTexture.drawText(material.text, 130, 600, "18px Arial", "white", "gray", true);
+ myDynamicTexture.drawText(color.uri, 130, 600, "18px Arial", "white", "gray", true);
+ basic.diffuseTexture = myDynamicTexture;
+ } else {
+ basic.bumpTexture = new Texture(color.uri, bScene_, false, false);
+ basic.emissiveTexture = new Texture(color.uri, bScene_, false, false);
+ basic.diffuseTexture = new Texture(color.uri, bScene_, false, false);
+ basic.diffuseTexture.coordinatesMode = Texture.FIXED_EQUIRECTANGULAR_MODE;
+ basic.backFaceCulling = false;
+ }
+ }
+ break;
+ }
+ }
+ }
+ bMaterial = basic;
+ break;
+ }
+ case 'pbr': {
+ const pbr = new PBRMaterial(id, bScene_);
+ const { albedo, ambient, emissive, metalness, reflection } = material;
+ if (albedo) {
+ switch (albedo.type) {
+ case 'color3': {
+ pbr.albedoColor = Color.toBabylon(albedo.color);
+ break;
+ }
+ case 'texture': {
+ pbr.albedoTexture = new Texture(albedo.uri, bScene_);
+ break;
+ }
+ }
+ }
+ if (ambient) {
+ switch (ambient.type) {
+ case 'color3': {
+ pbr.ambientColor = Color.toBabylon(ambient.color);
+ break;
+ }
+ case 'texture': {
+ pbr.ambientTexture = new Texture(ambient.uri, bScene_);
+ break;
+ }
+ }
+ }
+ if (emissive) {
+ const glow = new GlowLayer('glow', bScene_);
+ switch (emissive.type) {
+ case 'color3': {
+ pbr.emissiveColor = Color.toBabylon(emissive.color);
+ break;
+ }
+ case 'texture': {
+ pbr.emissiveTexture = new Texture(emissive.uri, bScene_);
+ break;
+ }
+ }
+ }
+
+ if (metalness) {
+ switch (metalness.type) {
+ case 'color1': {
+ pbr.metallic = metalness.color;
+ break;
+ }
+ case 'texture': {
+ pbr.metallicTexture = new Texture(metalness.uri, bScene_);
+ break;
+ }
+ }
+ }
+
+ if (reflection) {
+ switch (reflection.type) {
+ case 'color3': {
+ pbr.reflectivityColor = Color.toBabylon(reflection.color);
+ break;
+ }
+ case 'texture': {
+ pbr.reflectivityTexture = new Texture(reflection.uri, bScene_);
+ break;
+ }
+ }
+ }
+ bMaterial = pbr;
+ break;
+ }
+ }
+ return bMaterial;
+};
+
+export const updateMaterialBasic = (bMaterial: StandardMaterial, material: Patch.InnerPatch<Material.Basic>, bScene_: babylScene) => {
+ const { color } = material;
+ if (color.type === Patch.Type.InnerChange || color.type === Patch.Type.OuterChange) {
+ switch (color.next.type) {
+ case 'color3': {
+ bMaterial.diffuseColor = Color.toBabylon(color.next.color);
+ bMaterial.diffuseTexture = null;
+ break;
+ }
+ case 'texture': {
+ if (!color.next.uri) {
+ bMaterial.diffuseColor = new Color3(0.5, 0, 0.5);
+ bMaterial.diffuseTexture = null;
+ } else if (color.next.uri[0] !== '/') {
+ const myDynamicTexture = new DynamicTexture("dynamic texture", 1000, bScene_, true);
+ // myDynamicTexture.drawText(material.text, 130, 600, "18px Arial", "white", "gray", true);
+ myDynamicTexture.drawText(color.next.uri, 130, 600, "18px Arial", "white", "gray", true);
+ bMaterial.diffuseTexture = myDynamicTexture;
+ } else {
+ bMaterial.diffuseColor = Color.toBabylon(Color.WHITE);
+ bMaterial.diffuseTexture = new Texture(color.next.uri, bScene_);
+ }
+ break;
+ }
+ }
+ }
+ return bMaterial;
+};
+
+export const updateMaterialPbr = (bMaterial: PBRMaterial, material: Patch.InnerPatch<Material.Pbr>, bScene_: babylScene) => {
+ const { albedo, ambient, emissive, metalness, reflection } = material;
+ if (albedo.type === Patch.Type.OuterChange) {
+ switch (albedo.next.type) {
+ case 'color3': {
+ bMaterial.albedoColor = Color.toBabylon(albedo.next.color);
+ bMaterial.albedoTexture = null;
+ break;
+ }
+ case 'texture': {
+ if (!albedo.next.uri) {
+ bMaterial.albedoColor = new Color3(0.5, 0, 0.5);
+ } else {
+ bMaterial.albedoColor = Color.toBabylon(Color.WHITE);
+ bMaterial.albedoTexture = new Texture(albedo.next.uri, bScene_);
+ }
+ break;
+ }
+ }
+ }
+ if (ambient.type === Patch.Type.OuterChange) {
+ switch (ambient.next.type) {
+ case 'color3': {
+ bMaterial.ambientColor = Color.toBabylon(ambient.next.color);
+ bMaterial.ambientTexture = null;
+ break;
+ }
+ case 'texture': {
+ if (!ambient.next.uri) {
+ bMaterial.ambientColor = new Color3(0.5, 0, 0.5);
+ bMaterial.ambientTexture = null;
+ } else {
+ bMaterial.ambientColor = Color.toBabylon(Color.WHITE);
+ bMaterial.ambientTexture = new Texture(ambient.next.uri, bScene_);
+ }
+ break;
+ }
+ }
+ }
+ if (emissive.type === Patch.Type.OuterChange) {
+ switch (emissive.next.type) {
+ case 'color3': {
+ bMaterial.emissiveColor = Color.toBabylon(emissive.next.color);
+ bMaterial.emissiveTexture = null;
+ break;
+ }
+ case 'texture': {
+ if (!emissive.next.uri) {
+ bMaterial.emissiveColor = new Color3(0.5, 0, 0.5);
+ bMaterial.emissiveTexture = null;
+ } else {
+ bMaterial.emissiveColor = Color.toBabylon(Color.BLACK);
+ bMaterial.emissiveTexture = new Texture(emissive.next.uri, bScene_);
+ }
+ break;
+ }
+ }
+ }
+ if (metalness.type === Patch.Type.OuterChange) {
+ switch (metalness.next.type) {
+ case 'color1': {
+ bMaterial.metallic = metalness.next.color;
+ bMaterial.metallicTexture = null;
+ break;
+ }
+ case 'texture': {
+ if (!metalness.next.uri) {
+ bMaterial.metallic = 0;
+ } else {
+ bMaterial.metallicTexture = new Texture(metalness.next.uri, bScene_);
+ }
+ break;
+ }
+ }
+ }
+ if (reflection.type === Patch.Type.OuterChange) {
+ switch (reflection.next.type) {
+ case 'color3': {
+ bMaterial.reflectivityColor = Color.toBabylon(reflection.next.color);
+ bMaterial.reflectivityTexture = null;
+ break;
+ }
+ case 'texture': {
+ if (!reflection.next.uri) {
+ bMaterial.reflectivityColor = new Color3(0.5, 0, 0.5);
+ bMaterial.reflectivityTexture = null;
+ } else {
+ bMaterial.reflectivityColor = Color.toBabylon(Color.WHITE);
+ bMaterial.reflectivityTexture = new Texture(reflection.next.uri, bScene_);
+ }
+ break;
+ }
+ }
+ }
+
+ return bMaterial;
+};
+
diff --git a/src/Bindings/MotorBindings.ts b/src/Bindings/MotorBindings.ts
new file mode 100644
index 00000000..8ab78053
--- /dev/null
+++ b/src/Bindings/MotorBindings.ts
@@ -0,0 +1,80 @@
+
+import { Scene as babylScene, Quaternion, Vector3, Mesh,
+ PhysicsConstraintAxis, Physics6DoFConstraint,
+ HingeConstraint, PhysicsConstraintAxisLimitMode
+} from '@babylonjs/core';
+
+import Node from '../state/State/Robot/Node';
+import { Vector3wUnits } from '../util/unit-math';
+import { RENDER_SCALE } from '../components/Constants/renderConstants';
+import { RawVector3 } from '../util/math';
+
+
+
+
+// Adds a physics constraint between a parent and child link.
+export const createHinge_ = (id: string, hinge: Node.HingeJoint & { parentId: string }, bScene_: babylScene, bParent: Mesh, bChild: Mesh) => {
+ // Begin by moving the child in place (this prevents inertial snap as the physics engine applys the constraint)
+// const { bParent, bChild } = this.bParentChild_(id, hinge.parentId);
+ bChild.setParent(bParent);
+ bChild.position.x = Vector3wUnits.toBabylon(hinge.parentPivot, 'meters')._x;
+ bChild.position.y = Vector3wUnits.toBabylon(hinge.parentPivot, 'meters')._y;
+ bChild.position.z = Vector3wUnits.toBabylon(hinge.parentPivot, 'meters')._z;
+ bChild.rotationQuaternion = Quaternion.FromEulerAngles(hinge.parentAxis.z * 3.1415 / 2, 0, 0);
+
+ // The 6DoF constraint is used for motorized joints. Unforunately, it is not possible to
+ // completely lock these joints as hinges, so we also apply a hinge constraint.
+ // Order appears to matter here, the hinge should come before the 6DoF constraint.
+ if (id.includes("claw")) {
+ const hingeJoint = new HingeConstraint(
+ Vector3wUnits.toBabylon(hinge.parentPivot, RENDER_SCALE),
+ Vector3wUnits.toBabylon(hinge.childPivot, RENDER_SCALE),
+ new Vector3(0,0,1),
+ new Vector3(0,1,0),
+ bScene_
+ );
+ bParent.physicsBody.addConstraint(bChild.physicsBody, hingeJoint);
+ } else if (id.includes("arm")) {
+ const hingeJoint = new HingeConstraint(
+ Vector3wUnits.toBabylon(hinge.parentPivot, RENDER_SCALE),
+ Vector3wUnits.toBabylon(hinge.childPivot, RENDER_SCALE),
+ new Vector3(0,0,1),
+ new Vector3(0,-1,0),
+ bScene_
+ );
+ bParent.physicsBody.addConstraint(bChild.physicsBody, hingeJoint);
+ } else if (id.includes("wheel")) {
+ const hingeJoint = new HingeConstraint(
+ Vector3wUnits.toBabylon(hinge.parentPivot, RENDER_SCALE),
+ Vector3wUnits.toBabylon(hinge.childPivot, RENDER_SCALE),
+ new Vector3(0,1,0),
+ undefined,
+ bScene_
+ );
+ bParent.physicsBody.addConstraint(bChild.physicsBody, hingeJoint);
+ }
+ const joint: Physics6DoFConstraint = new Physics6DoFConstraint({
+ pivotA: Vector3wUnits.toBabylon(hinge.parentPivot, RENDER_SCALE),
+ pivotB: Vector3wUnits.toBabylon(hinge.childPivot, RENDER_SCALE),
+ axisA: new Vector3(1,0,0),
+ axisB: new Vector3(1,0,0),
+ perpAxisA: new Vector3(0,-1,0), // bChildAxis, //
+ perpAxisB: RawVector3.toBabylon(hinge.parentAxis),
+ },
+ [
+ {
+ axis: PhysicsConstraintAxis.ANGULAR_Z,
+ minLimit: -30 * Math.PI / 180, maxLimit: -30 * Math.PI / 180,
+ }
+ ],
+ bScene_
+ );
+
+ bParent.physicsBody.addConstraint(bChild.physicsBody, joint);
+ joint.setAxisMode(PhysicsConstraintAxis.LINEAR_X, PhysicsConstraintAxisLimitMode.LOCKED);
+ joint.setAxisMode(PhysicsConstraintAxis.LINEAR_Y, PhysicsConstraintAxisLimitMode.LOCKED);
+ joint.setAxisMode(PhysicsConstraintAxis.LINEAR_Z, PhysicsConstraintAxisLimitMode.LOCKED);
+ joint.setAxisMode(PhysicsConstraintAxis.ANGULAR_X, PhysicsConstraintAxisLimitMode.LOCKED);
+ joint.setAxisMode(PhysicsConstraintAxis.ANGULAR_Y, PhysicsConstraintAxisLimitMode.LOCKED);
+ return joint;
+};
\ No newline at end of file
diff --git a/src/Bindings/ObjectBinding.ts b/src/Bindings/ObjectBinding.ts
new file mode 100644
index 00000000..4f3bda06
--- /dev/null
+++ b/src/Bindings/ObjectBinding.ts
@@ -0,0 +1,169 @@
+import { PhysicsShapeType, IPhysicsCollisionEvent, IPhysicsEnginePluginV2, PhysicsAggregate,
+ TransformNode, AbstractMesh, PhysicsViewer, ShadowGenerator, CreateBox, CreateSphere, CreateCylinder,
+ CreatePlane, Vector4, Vector3, Texture, DynamicTexture, StandardMaterial, GizmoManager, ArcRotateCamera,
+ IShadowLight, PointLight, SpotLight, DirectionalLight, Color3, PBRMaterial, Mesh, SceneLoader, EngineView,
+ Scene as babylScene, Node as babylNode, Camera as babylCamera, Material as babylMaterial,
+ GlowLayer, Observer, BoundingBox } from '@babylonjs/core';
+
+import Geometry from "../state/State/Scene/Geometry";
+import { RawQuaternion, RawVector2, RawVector3 } from "../util/math";
+import { Angle, Distance, Mass, SetOps } from "../util";
+import Node from "../state/State/Scene/Node";
+import Scene from "../state/State/Scene";
+import LocalizedString from '../util/LocalizedString';
+import { apply } from './helpers';
+import { createMaterial } from './MaterialBinding';
+import { preBuiltGeometries, preBuiltTemplates } from "../NodeTemplates";
+
+export type FrameLike = TransformNode | AbstractMesh;
+
+let seed_ = 1;
+
+const random = (max: number, min: number) => {
+ let x = Math.sin(seed_++) * 10000;
+ x = x - Math.floor(x);
+ x = ((x - .5) * (max - min)) + ((max + min) / 2);
+ return x;
+};
+
+export const buildGeometryFaceUvs = (faceUvs: RawVector2[] | undefined, expectedUvs: number): Vector4[] => {
+ if (faceUvs?.length !== expectedUvs) {
+ return undefined;
+ }
+ const ret: Vector4[] = [];
+ for (let i = 0; i + 1 < faceUvs.length; i += 2) {
+ ret.push(new Vector4(faceUvs[i].x, faceUvs[i].y, faceUvs[i + 1].x, faceUvs[i + 1].y));
+ }
+ return ret;
+};
+
+export const buildGeometry = async (name: string, geometry: Geometry, bScene_: babylScene, faceUvs?: RawVector2[]): Promise<FrameLike> => {
+ let ret: FrameLike;
+ switch (geometry.type) {
+ case 'box': {
+ const rect = CreateBox(name, {
+ updatable:true,
+ width: Distance.toCentimetersValue(geometry.size.x),
+ height: Distance.toCentimetersValue(geometry.size.y),
+ depth: Distance.toCentimetersValue(geometry.size.z),
+ faceUV: buildGeometryFaceUvs(faceUvs, 12),
+ }, bScene_);
+ const verts = rect.getVerticesData("position");
+ ret = rect;
+ break;
+ }
+ case 'sphere': {
+ const bFaceUvs = buildGeometryFaceUvs(faceUvs, 2)?.[0];
+ const segments = 4;
+ const rock = CreateSphere(name, {
+ segments: segments,
+ updatable:true,
+ frontUVs: bFaceUvs,
+ sideOrientation: bFaceUvs ? Mesh.DOUBLESIDE : undefined,
+ diameterX:Distance.toCentimetersValue(geometry.radius) * 2,
+ diameterY:Distance.toCentimetersValue(geometry.radius) * 2 * geometry.squash,
+ diameterZ:Distance.toCentimetersValue(geometry.radius) * 2 * geometry.stretch,
+ }, bScene_);
+
+ const positions = rock.getVerticesData("position");
+ // TODO: Replace with custom rocks from blender
+ if (name.includes('Rock')) {
+ const skip = [25,26,38,39,51,52,64,65];
+ for (let i = 14; i < 65; i++) {
+ if (skip.includes(i)) {
+ continue;
+ } else {
+ positions[3 * i] = positions[3 * i] + random(geometry.noise, -1 * geometry.noise);
+ positions[1 + 3 * i] = positions[1 + 3 * i] + random(geometry.noise, -1 * geometry.noise);
+ positions[2 + 3 * i] = positions[2 + 3 * i] + random(geometry.noise, -1 * geometry.noise);
+ }
+ }
+ }
+ rock.updateVerticesData("position", positions);
+ ret = rock;
+ break;
+ }
+ case 'cylinder': {
+ ret = CreateCylinder(name, {
+ height: Distance.toCentimetersValue(geometry.height),
+ diameterTop: Distance.toCentimetersValue(geometry.radius) * 2,
+ diameterBottom: Distance.toCentimetersValue(geometry.radius) * 2,
+ faceUV: buildGeometryFaceUvs(faceUvs, 6),
+ }, bScene_);
+ break;
+ }
+ case 'cone': {
+ ret = CreateCylinder(name, {
+ diameterTop: 0,
+ height: Distance.toCentimetersValue(geometry.height),
+ diameterBottom: Distance.toCentimetersValue(geometry.radius) * 2,
+ faceUV: buildGeometryFaceUvs(faceUvs, 6),
+ }, bScene_);
+ break;
+ }
+ case 'plane': {
+ ret = CreatePlane(name, {
+ width: Distance.toCentimetersValue(geometry.size.x),
+ height: Distance.toCentimetersValue(geometry.size.y),
+ frontUVs: buildGeometryFaceUvs(faceUvs, 2)?.[0],
+ }, bScene_);
+ break;
+ }
+ case 'file': {
+ const index = geometry.uri.lastIndexOf('/');
+ const fileName = geometry.uri.substring(index + 1);
+ const baseName = geometry.uri.substring(0, index + 1);
+
+ const res = await SceneLoader.ImportMeshAsync(geometry.include ?? '', baseName, fileName, bScene_);
+ if (res.meshes.length === 1) return res.meshes[0];
+ // const nonColliders: Mesh[] = [];
+ ret = new TransformNode(geometry.uri, bScene_);
+ for (const mesh of res.meshes as Mesh[]) {
+ // GLTF importer adds a __root__ mesh (always the first one) that we can ignore
+ if (mesh.name === '__root__') continue;
+ // nonColliders.push(mesh);
+ mesh.setParent(ret);
+ }
+ // const mesh = Mesh.MergeMeshes(nonColliders, true, true, undefined, false, true);
+ break;
+ }
+ default: {
+ throw new Error(`Unsupported geometry type: ${geometry.type}`);
+ }
+ }
+ if (ret instanceof AbstractMesh) {
+ ret.visibility = 1;
+ } else {
+ const children = ret.getChildren(c => c instanceof AbstractMesh) as Mesh[];
+ const mesh = Mesh.MergeMeshes(children, true, true, undefined, false, true);
+ mesh.visibility = 1;
+ ret = mesh;
+ }
+ return ret;
+};
+
+export const createObject = async (node: Node.Obj, nextScene: Scene, parent: babylNode, bScene_: babylScene): Promise<babylNode> => {
+
+ const geometry = nextScene.geometry[node.geometryId] ?? preBuiltGeometries[node.geometryId];
+ if (!geometry) {
+ console.error(`node ${LocalizedString.lookup(node.name, LocalizedString.EN_US)} has invalid geometry ID: ${node.geometryId}`);
+ return null;
+ }
+ const ret = await buildGeometry(node.name[LocalizedString.EN_US], geometry, bScene_, node.faceUvs);
+ if (!node.visible) {
+ apply(ret, m => m.isVisible = false);
+ }
+ if (node.material) {
+ const material = createMaterial(node.name[LocalizedString.EN_US], node.material, bScene_);
+ apply(ret, m => m.material = material);
+ }
+ ret.setParent(parent);
+ return ret;
+};
+
+export const createEmpty = (node: Node.Empty, parent: babylNode,bScene_: babylScene): TransformNode => {
+
+ const ret = new TransformNode(node.name[LocalizedString.EN_US], bScene_);
+ ret.setParent(parent);
+ return ret;
+};
diff --git a/src/RobotBinding.ts b/src/Bindings/RobotBinding.ts
similarity index 73%
rename from src/RobotBinding.ts
rename to src/Bindings/RobotBinding.ts
index b341d31b..9f85399f 100644
--- a/src/RobotBinding.ts
+++ b/src/Bindings/RobotBinding.ts
@@ -9,34 +9,21 @@ import { Scene as babylScene, TransformNode, AbstractMesh, LinesMesh, PhysicsVie
import '@babylonjs/core/Physics/physicsEngineComponent';
-import SceneNode from './state/State/Scene/Node';
-import Robot from './state/State/Robot';
-import Node from './state/State/Robot/Node';
-import { RawQuaternion, RawVector3, clamp, RawEuler } from './util/math';
-import { ReferenceFramewUnits, RotationwUnits, Vector3wUnits } from './util/unit-math';
-import { Angle, Distance, Mass } from './util/Value';
+import SceneNode from '../state/State/Scene/Node';
+import Robot from '../state/State/Robot';
+import Node from '../state/State/Robot/Node';
+import { RawQuaternion, RawVector3, clamp, RawEuler } from '../util/math';
+import { ReferenceFramewUnits, RotationwUnits, Vector3wUnits } from '../util/unit-math';
+import { Angle, Distance, Mass } from '../util/Value';
import { SceneMeshMetadata } from './SceneBinding';
-import Geometry from './state/State/Robot/Geometry';
-import Dict from './util/Dict';
-import { RENDER_SCALE, RENDER_SCALE_METERS_MULTIPLIER } from './components/Constants/renderConstants';
-import WriteCommand from './AbstractRobot/WriteCommand';
-import AbstractRobot from './AbstractRobot';
-import Motor from './AbstractRobot/Motor';
-import { node } from 'prop-types';
-
-interface BuiltGeometry {
- nonColliders: Mesh[];
- colliders?: BuiltGeometry.Collider[];
-}
-
-namespace BuiltGeometry {
- export interface Collider {
- name: string;
- mesh: Mesh;
- type: number;
- volume: number;
- }
-}
+import Dict from '../util/Dict';
+import { RENDER_SCALE, RENDER_SCALE_METERS_MULTIPLIER } from '../components/Constants/renderConstants';
+import WriteCommand from '../AbstractRobot/WriteCommand';
+import AbstractRobot from '../AbstractRobot';
+import Motor from '../AbstractRobot/Motor';
+import { createLink_ } from './RobotLink';
+import { createHinge_ } from './MotorBindings';
+import { createWeight_ } from './WeightBinding';
class RobotBinding {
private bScene_: babylScene;
@@ -70,175 +57,30 @@ class RobotBinding {
private physicsViewer_: PhysicsViewer;
- constructor(bScene: babylScene, physicsViewer?: PhysicsViewer) {
- this.bScene_ = bScene;
- this.physicsViewer_ = physicsViewer;
- }
- // Loads the geometry of a robot part and divides into the collider and noncollider pieces
- private buildGeometry_ = async (name: string, geometry: Geometry): Promise<BuiltGeometry> => {
- let ret: BuiltGeometry;
-
- switch (geometry.type) {
- case 'remote-mesh': {
- const index = geometry.uri.lastIndexOf('/');
- const fileName = geometry.uri.substring(index + 1);
- const baseName = geometry.uri.substring(0, index + 1);
-
- const res = await SceneLoader.ImportMeshAsync(geometry.include ?? '', baseName, fileName, this.bScene_);
-
- const nonColliders: Mesh[] = [];
- const colliders: BuiltGeometry.Collider[] = [];
-
- for (const mesh of res.meshes.slice(1) as Mesh[]) {
-
- // The robot mesh includes sub-meshes with the 'collider' name to indicate their use.
- if (mesh.name.startsWith('collider')) {
- const parts = mesh.name.split('-');
- if (parts.length !== 3) throw new Error(`Invalid collider name: ${mesh.name}`);
- const [_, type, name] = parts;
-
- const { extendSize } = mesh.getBoundingInfo().boundingBox;
-
- const volume = extendSize.x * extendSize.y * extendSize.z;
-
- let bType: number;
- switch (type) {
- case 'box': bType = PhysicsShapeType.BOX; break;
- case 'sphere': bType = PhysicsShapeType.SPHERE; break;
- case 'cylinder': bType = PhysicsShapeType.CYLINDER; break;
- case 'capsule': bType = PhysicsShapeType.CAPSULE; break;
- case 'plane': bType = PhysicsShapeType.HEIGHTFIELD; break;
- case 'mesh': bType = PhysicsShapeType.MESH; break;
- default: throw new Error(`Invalid collider type: ${type}`);
- }
- colliders.push({ mesh, type: bType, name, volume });
- } else {
- nonColliders.push(mesh);
- }
- }
+ private lastTick_ = 0;
+ private lastMotorAngles_: [number, number, number, number] = [0, 0, 0, 0];
- ret = { nonColliders, colliders };
- break;
- }
- default: { throw new Error(`Unsupported geometry type: ${geometry.type}`); }
- }
- return ret;
- };
+ // Getting sensor values is async. We store the pending promises in these dictionaries.
+ private outstandingDigitalGetValues_: Dict<RobotBinding.OutstandingPromise<boolean>> = {};
+ private outstandingAnalogGetValues_: Dict<RobotBinding.OutstandingPromise<number>> = {};
+ private latestDigitalValues_: [boolean, boolean, boolean, boolean, boolean, boolean] = [false, false, false, false, false, false];
+ private latestAnalogValues_: [number, number, number, number, number, number] = [0, 0, 0, 0, 0, 0];
- // Creates a link and returns the root mesh. Links are wheels, chasis, arms, etc.
- // Loads the geometry and adds the appropriate physics properties to the mesh
- private createLink_ = async (id: string, link: Node.Link) => {
+ private lastPErrs_: [number, number, number, number] = [0, 0, 0, 0];
+ private iErrs_: [number, number, number, number] = [0, 0, 0, 0];
- let builtGeometry: BuiltGeometry;
- if (link.geometryId === undefined) {
- builtGeometry = { nonColliders: [new Mesh(id, this.bScene_)] };
- } else {
- const geometry = this.robot_.geometry[link.geometryId];
- if (!geometry) throw new Error(`Missing geometry: ${link.geometryId}`);
- builtGeometry = await this.buildGeometry_(id, geometry);
- }
-
- const meshes = builtGeometry.nonColliders;
- let myMesh: Mesh;
+ private brakeAt_: [number, number, number, number] = [undefined, undefined, undefined, undefined];
- const inertiaScale = .5;
-
- switch (link.collisionBody.type) {
- // Notes on Links - the root link should have the highest mass and inertia and it should
- // scale down further out the tree to prevent wild oscillations.
- // body.setMassProperties can also help setting the inertia vector,
- // body.setAngularDamping and body.setLinearDamping can help with oscillations
-
- case Node.Link.CollisionBody.Type.Box: {
- // alert("box collision body"); // Currently there are no box collision bodies in the robot model
- myMesh = Mesh.MergeMeshes(meshes, true, true, undefined, false, true);
- myMesh.scaling.scaleInPlace(RENDER_SCALE_METERS_MULTIPLIER);
-
- const aggregate = new PhysicsAggregate(myMesh, PhysicsShapeType.BOX, {
- mass: Mass.toGramsValue(link.mass || Mass.grams(10)),
- friction: link.friction ?? 0.5,
- restitution: link.restitution ?? 0,
- startAsleep: true,
- }, this.bScene_);
-
- this.colliders_.add(myMesh);
- break;
- }
- case Node.Link.CollisionBody.Type.Cylinder: {
- myMesh = Mesh.MergeMeshes(meshes, true, true, undefined, false, true);
- myMesh.scaling.scaleInPlace(RENDER_SCALE_METERS_MULTIPLIER);
-
- const aggregate = new PhysicsAggregate(myMesh, PhysicsShapeType.CYLINDER, {
- mass: Mass.toGramsValue(link.mass || Mass.grams(10)),
- friction: link.friction ?? 0.5,
- restitution: link.restitution ?? 0,
- startAsleep: true,
- }, this.bScene_);
-
- this.colliders_.add(myMesh);
- break;
- }
- case Node.Link.CollisionBody.Type.Embedded: {
- myMesh = Mesh.MergeMeshes(meshes, true, true, undefined, false, true);
- myMesh.scaling.scaleInPlace(RENDER_SCALE_METERS_MULTIPLIER);
-
- // As the embedded collision body consists of multiple meshes, we need to create a parent
- // This mmeans we are unable to use the physics aggregate
- const parentShape = new PhysicsShapeContainer(this.bScene_);
-
- for (const collider of builtGeometry.colliders ?? []) {
- const bCollider = collider.mesh;
- bCollider.parent = myMesh;
-
- const parameters: PhysicsShapeParameters = { mesh: bCollider };
- const options: PhysicShapeOptions = { type: PhysicsShapeType.MESH, parameters: parameters };
- const shape = new PhysicsShape(options, this.bScene_);
- shape.material = {
- friction: link.friction ?? 0.5,
- restitution: link.restitution ?? 0.1,
- };
-
- parentShape.addChild(shape, bCollider.absolutePosition, bCollider.absoluteRotationQuaternion);
-
- bCollider.visibility = 0;
- this.colliders_.add(bCollider);
- }
-
- const body = new PhysicsBody(myMesh, PhysicsMotionType.DYNAMIC, false, this.bScene_);
- body.shape = parentShape;
- if (link.geometryId.includes("chassis")) {
- body.setMassProperties({
- mass: Mass.toGramsValue(link.mass || Mass.grams(100)),
- inertia: new Vector3(10 * inertiaScale,10 * inertiaScale,10 * inertiaScale) // (left/right, twist around, rock forward and backward)
- });
- }
- if (link.geometryId.includes("arm")) {
- body.setMassProperties({
- mass: Mass.toGramsValue(link.mass || Mass.grams(80)),
- inertia: new Vector3(6 * inertiaScale,6 * inertiaScale,6 * inertiaScale) // (left/right, twist around, rock forward and backward)
- });
- }
- if (link.geometryId.includes("claw")) {
- body.setMassProperties({
- mass: Mass.toGramsValue(link.mass || Mass.grams(10)),
- inertia: new Vector3(3 * inertiaScale,3 * inertiaScale,3 * inertiaScale) // (left/right, twist around, rock forward and backward)
- });
- }
- body.setAngularDamping(.5);
-
- this.colliders_.add(myMesh);
+ private positionDeltaFracs_: [number, number, number, number] = [0, 0, 0, 0];
+ private lastServoEnabledAngle_: [number, number, number, number] = [0, 0, 0, 0];
+
+ constructor(bScene: babylScene, physicsViewer?: PhysicsViewer) {
+ this.bScene_ = bScene;
+ this.physicsViewer_ = physicsViewer;
+ }
- break;
- }
- default: {
- throw new Error(`Unsupported collision body type: ${link.collisionBody.type}`);
- }
- }
- if (this.physicsViewer_ && myMesh.physicsBody) this.physicsViewer_.showBody(myMesh.physicsBody);
- return myMesh;
- };
private createSensor_ = <T extends Node.FrameLike, O, S extends RobotBinding.SensorObject<T, O>>(s: { new(parameters: RobotBinding.SensorParameters<T>): S }) => (id: string, definition: T): S => {
const parent = this.links_[definition.parentId];
@@ -253,67 +95,31 @@ class RobotBinding {
});
};
+ // These return functions are used to create sensors of a specific type.
private createTouchSensor_ = this.createSensor_(RobotBinding.TouchSensor);
private createEtSensor_ = this.createSensor_(RobotBinding.EtSensor);
private createLightSensor_ = this.createSensor_(RobotBinding.LightSensor);
private createReflectanceSensor_ = this.createSensor_(RobotBinding.ReflectanceSensor);
- // Transform a vector using the child frame's orientation. This operation is invariant on a single
- // axis, so we return a new quaternion with the leftover rotation.
- private static connectedAxisAngle_ = (rotationAxis: Vector3, childOrientation: Quaternion): { axis: Vector3, twist: Quaternion } => {
- const childOrientationInv = childOrientation.invert();
- const axis = rotationAxis.applyRotationQuaternion(childOrientationInv);
- const v = new Vector3(childOrientationInv.x, childOrientationInv.y, childOrientationInv.z);
- const s = childOrientationInv.w;
- v.multiplyInPlace(axis);
-
- const twist = new Quaternion(v.x, v.y, v.z, s);
- twist.normalize();
-
-
- return {
- axis,
- twist,
- };
- };
-
- // Adds an invisible weight to a parent link.
- private createWeight_ = (id: string, weight: Node.Weight) => {
- const ret = CreateSphere(id, { diameter: 1 }, this.bScene_);
- ret.visibility = 0;
-
- const parent = this.robot_.nodes[weight.parentId];
- if (!parent) throw new Error(`Missing parent: "${weight.parentId}" for weight "${id}"`);
- if (parent.type !== Node.Type.Link) throw new Error(`Invalid parent type: "${parent.type}" for weight "${id}"`);
-
- const aggregate = new PhysicsAggregate(ret, PhysicsShapeType.CYLINDER, {
- mass: Mass.toGramsValue(weight.mass),
- friction: 0,
- restitution: 0,
- }, this.bScene_);
-
- const bParent = this.links_[weight.parentId];
- if (!bParent) throw new Error(`Missing parent instantiation: "${weight.parentId}" for weight "${id}"`);
-
- const bOrigin = ReferenceFramewUnits.toBabylon(weight.origin, RENDER_SCALE);
-
- const constraint = new LockConstraint(
- bOrigin.position,
- new Vector3(0,0,0), // RawVector3.toBabylon(new Vector3(-8,10,0)), // updown, frontback
- Vector3.Up(),
- Vector3.Up().applyRotationQuaternion(bOrigin.rotationQuaternion.invert()),
- this.bScene_
- );
- bParent.physicsBody.addConstraint(ret.physicsBody, constraint);
+ set realisticSensors(realisticSensors: boolean) {
+ for (const digitalSensor of Object.values(this.digitalSensors_)) {
+ digitalSensor.realistic = realisticSensors;
+ }
+ for (const analogSensor of Object.values(this.analogSensors_)) {
+ analogSensor.realistic = realisticSensors;
+ }
+ }
- return ret;
- };
+ set noisySensors(noisySensors: boolean) {
+ for (const digitalSensor of Object.values(this.digitalSensors_)) {
+ digitalSensor.noisy = noisySensors;
+ }
+ for (const analogSensor of Object.values(this.analogSensors_)) {
+ analogSensor.noisy = noisySensors;
+ }
+ }
- private bParentChild_ = (id: string, parentId: string): {
- bParent: Mesh;
- bChild: Mesh;
- childId: string;
- } => {
+ private bParentChild_ = (id: string, parentId: string): { bParent: Mesh; bChild: Mesh; childId: string; } => {
if (!parentId) throw new Error(`Missing parent: "${parentId}" for node "${id}"`);
const children = this.childrenNodeIds_[id];
@@ -334,107 +140,25 @@ class RobotBinding {
};
};
- // Adds a physics constraint between a parent and child link.
- private createHinge_ = (id: string, hinge: Node.HingeJoint & { parentId: string }) => {
-
- // Begin by moving the child in place (this prevents inertial snap as the physics engine applys the constraint)
- const { bParent, bChild } = this.bParentChild_(id, hinge.parentId);
- bChild.setParent(bParent);
- bChild.position.x = Vector3wUnits.toBabylon(hinge.parentPivot, 'meters')._x;
- bChild.position.y = Vector3wUnits.toBabylon(hinge.parentPivot, 'meters')._y;
- bChild.position.z = Vector3wUnits.toBabylon(hinge.parentPivot, 'meters')._z;
-
- bChild.rotationQuaternion = Quaternion.FromEulerAngles(hinge.parentAxis.z * 3.1415 / 2, 0, 0);
-
- // The 6DoF constraint is used for motorized joints. Unforunately, it is not possible to
- // completely lock these joints as hinges, so we also apply a hinge constraint.
- // Order appears to matter here, the hinge should come before the 6DoF constraint.
- if (id.includes("claw")) {
- const hingeJoint = new HingeConstraint(
- Vector3wUnits.toBabylon(hinge.parentPivot, RENDER_SCALE),
- Vector3wUnits.toBabylon(hinge.childPivot, RENDER_SCALE),
- new Vector3(0,0,1),
- new Vector3(0,1,0),
- this.bScene_
- );
- bParent.physicsBody.addConstraint(bChild.physicsBody, hingeJoint);
- } else if (id.includes("arm")) {
- const hingeJoint = new HingeConstraint(
- Vector3wUnits.toBabylon(hinge.parentPivot, RENDER_SCALE),
- Vector3wUnits.toBabylon(hinge.childPivot, RENDER_SCALE),
- new Vector3(0,0,1),
- new Vector3(0,-1,0),
- this.bScene_
- );
- bParent.physicsBody.addConstraint(bChild.physicsBody, hingeJoint);
- } else if (id.includes("wheel")) {
- const hingeJoint = new HingeConstraint(
- Vector3wUnits.toBabylon(hinge.parentPivot, RENDER_SCALE),
- Vector3wUnits.toBabylon(hinge.childPivot, RENDER_SCALE),
- new Vector3(0,1,0),
- undefined,
- this.bScene_
- );
- bParent.physicsBody.addConstraint(bChild.physicsBody, hingeJoint);
- }
- const joint: Physics6DoFConstraint = new Physics6DoFConstraint({
- pivotA: Vector3wUnits.toBabylon(hinge.parentPivot, RENDER_SCALE),
- pivotB: Vector3wUnits.toBabylon(hinge.childPivot, RENDER_SCALE),
- axisA: new Vector3(1,0,0),
- axisB: new Vector3(1,0,0),
- perpAxisA: new Vector3(0,-1,0), // bChildAxis, //
- perpAxisB: RawVector3.toBabylon(hinge.parentAxis),
- },
- [
- {
- axis: PhysicsConstraintAxis.ANGULAR_Z,
- minLimit: -30 * Math.PI / 180, maxLimit: -30 * Math.PI / 180,
- }
- ],
- this.bScene_
- );
-
- bParent.physicsBody.addConstraint(bChild.physicsBody, joint);
- joint.setAxisMode(PhysicsConstraintAxis.LINEAR_X, PhysicsConstraintAxisLimitMode.LOCKED);
- joint.setAxisMode(PhysicsConstraintAxis.LINEAR_Y, PhysicsConstraintAxisLimitMode.LOCKED);
- joint.setAxisMode(PhysicsConstraintAxis.LINEAR_Z, PhysicsConstraintAxisLimitMode.LOCKED);
- joint.setAxisMode(PhysicsConstraintAxis.ANGULAR_X, PhysicsConstraintAxisLimitMode.LOCKED);
- joint.setAxisMode(PhysicsConstraintAxis.ANGULAR_Y, PhysicsConstraintAxisLimitMode.LOCKED);
-
-
- return joint;
- };
-
- // Assumes the current hinge angle is the last target of the axis motor.
- // TODO: implement a method using relative poses to get the actual angle.
- private hingeAngle_ = (joint: Physics6DoFConstraint): number => {
- const currentAngle: number = joint.getAxisMotorTarget(0);
- return currentAngle;
- };
-
-
-
- private lastTick_ = 0;
- private lastMotorAngles_: [number, number, number, number] = [0, 0, 0, 0];
-
- // Getting sensor values is async. We store the pending promises in these dictionaries.
- private outstandingDigitalGetValues_: Dict<RobotBinding.OutstandingPromise<boolean>> = {};
- private outstandingAnalogGetValues_: Dict<RobotBinding.OutstandingPromise<number>> = {};
-
- private latestDigitalValues_: [boolean, boolean, boolean, boolean, boolean, boolean] = [false, false, false, false, false, false];
- private latestAnalogValues_: [number, number, number, number, number, number] = [0, 0, 0, 0, 0, 0];
-
- private lastPErrs_: [number, number, number, number] = [0, 0, 0, 0];
- private iErrs_: [number, number, number, number] = [0, 0, 0, 0];
-
- private brakeAt_: [number, number, number, number] = [undefined, undefined, undefined, undefined];
-
- private positionDeltaFracs_: [number, number, number, number] = [0, 0, 0, 0];
- private lastServoEnabledAngle_: [number, number, number, number] = [0, 0, 0, 0];
-
-
+ // Transform a vector using the child frame's orientation. This operation is invariant on a single
+ // axis, so we return a new quaternion with the leftover rotation.
+ // private static connectedAxisAngle_ = (rotationAxis: Vector3, childOrientation: Quaternion): { axis: Vector3, twist: Quaternion } => {
+ // const childOrientationInv = childOrientation.invert();
+ // const axis = rotationAxis.applyRotationQuaternion(childOrientationInv);
+ // const v = new Vector3(childOrientationInv.x, childOrientationInv.y, childOrientationInv.z);
+ // const s = childOrientationInv.w;
+ // v.multiplyInPlace(axis);
+ // const twist = new Quaternion(v.x, v.y, v.z, s);
+ // twist.normalize();
+
+ // return {
+ // axis,
+ // twist,
+ // };
+ // };
tick(readable: AbstractRobot.Readable): RobotBinding.TickOut {
+ // TODO: Motor Bindings need to be cleaned up to handle motor position explicitly.
const motorPositionDeltas: [number, number, number, number] = [0, 0, 0, 0];
const writeCommands: WriteCommand[] = [];
@@ -467,7 +191,10 @@ class RobotBinding {
const plug = (motorNode.plug === undefined || motorNode.plug === 'normal') ? 1 : -1;
// Get the delta angle of the motor since the last tick.
- const currentAngle = this.hingeAngle_(bMotor);
+ // Assumes the current hinge angle is the last target of the axis motor.
+ // TODO: implement a method using relative poses to get the actual angle.
+ const currentAngle: number = bMotor.getAxisMotorTarget(0);
+ // const currentAngle = this.hingeAngle_(bMotor);
const lastMotorAngle = this.lastMotorAngles_[port];
let deltaAngle = 0;
if (lastMotorAngle > Math.PI / 2 && currentAngle < -Math.PI / 2) {
@@ -674,6 +401,7 @@ class RobotBinding {
writeCommands.push(WriteCommand.digitalIn({ port: i, value: digitalValues[i] }));
}
+ // Analog Sensors
const analogValues: AbstractRobot.Stateless.AnalogValues = [0, 0, 0, 0, 0, 0];
for (let i = 0; i < 6; ++i) {
const analogId = this.analogPorts_[i];
@@ -831,46 +559,46 @@ class RobotBinding {
this.robot_ = robot;
+ // Set Root
const rootIds = Robot.rootNodeIds(robot);
if (rootIds.length !== 1) throw new Error('Only one root node is supported');
-
this.rootId_ = rootIds[0];
-
- const nodeIds = Robot.breadthFirstNodeIds(robot);
- this.childrenNodeIds_ = Robot.childrenNodeIds(robot);
-
const rootNode = robot.nodes[this.rootId_];
-
if (robot.nodes[this.rootId_].type !== Node.Type.Link) throw new Error('Root node must be a link');
+ const nodeIds = Robot.breadthFirstNodeIds(robot);
+ this.childrenNodeIds_ = Robot.childrenNodeIds(robot);
+ // Links need to be set up before the rest of the nodes
for (const nodeId of nodeIds) {
const node = robot.nodes[nodeId];
if (node.type !== Node.Type.Link) continue;
- const bNode = await this.createLink_(nodeId, node);
+ const bNode = await createLink_(nodeId, node, this.bScene_, this.robot_, this.colliders_);
+ if (this.physicsViewer_ && bNode.physicsBody) this.physicsViewer_.showBody(bNode.physicsBody);
+
bNode.metadata = { id: this.robotSceneId_, selected: false } as SceneMeshMetadata;
this.links_[nodeId] = bNode;
}
+ // Set up all other types of nodes
for (const nodeId of nodeIds) {
const node = robot.nodes[nodeId];
if (node.type === Node.Type.Link) continue;
switch (node.type) {
case Node.Type.Weight: {
- const bNode = this.createWeight_(nodeId, node);
+ const bNode = createWeight_(nodeId, node, this.bScene_, this.robot_, this.links_);
this.weights_[nodeId] = bNode;
break;
}
case Node.Type.Motor: {
- const bJoint = this.createHinge_(nodeId, node);
+ const { bParent, bChild } = this.bParentChild_(nodeId, node.parentId);
+ const bJoint = createHinge_(nodeId, node, this.bScene_, bParent, bChild);
+
bJoint.setAxisMotorMaxForce(PhysicsConstraintAxis.ANGULAR_Z, 1000000000);
- bJoint.setAxisMaxLimit(PhysicsConstraintAxis.ANGULAR_Z, 1000000000000);
- bJoint.setAxisMinLimit(PhysicsConstraintAxis.ANGULAR_Z, -1000000000000);
- bJoint.setAxisMotorTarget(PhysicsConstraintAxis.ANGULAR_Z, 0);
- bJoint.setAxisMotorType(PhysicsConstraintAxis.ANGULAR_Z, PhysicsConstraintMotorType.VELOCITY); // Position control
-
+ bJoint.setAxisMotorType(PhysicsConstraintAxis.ANGULAR_Z, PhysicsConstraintMotorType.VELOCITY);
+ // Start motor in locked position so the wheels don't slide
bJoint.setAxisMode(PhysicsConstraintAxis.ANGULAR_Z, PhysicsConstraintAxisLimitMode.LOCKED);
this.motors_[nodeId] = bJoint;
@@ -880,17 +608,17 @@ class RobotBinding {
case Node.Type.Servo: {
// minLimit: -30 * Math.PI / 180, maxLimit: -30 * Math.PI / 180,
// -90 is upright and closed; 0 is forward and open
- const bJoint = this.createHinge_(nodeId, node);
+ const { bParent, bChild } = this.bParentChild_(nodeId, node.parentId);
+ const bJoint = createHinge_(nodeId, node, this.bScene_, bParent, bChild);
bJoint.setAxisMotorMaxForce(PhysicsConstraintAxis.ANGULAR_Z, 10000000);
- bJoint.setAxisMotorTarget(PhysicsConstraintAxis.ANGULAR_Z, 2);
- bJoint.setAxisMotorType(PhysicsConstraintAxis.ANGULAR_Z, PhysicsConstraintMotorType.VELOCITY); // Velocity control
-
+ bJoint.setAxisMotorType(PhysicsConstraintAxis.ANGULAR_Z, PhysicsConstraintMotorType.VELOCITY);
+ // Start the servos at 0
bJoint.setAxisMaxLimit(PhysicsConstraintAxis.ANGULAR_Z, Angle.toRadiansValue(Angle.degrees(0)));
- bJoint.setAxisMinLimit(PhysicsConstraintAxis.ANGULAR_Z, Angle.toRadiansValue(Angle.degrees(-10)));
+ bJoint.setAxisMinLimit(PhysicsConstraintAxis.ANGULAR_Z, Angle.toRadiansValue(Angle.degrees(-1)));
this.servos_[nodeId] = bJoint;
this.servoPorts_[node.servoPort] = nodeId;
- this.lastServoEnabledAngle_[node.servoPort] = Angle.toRadiansValue(Angle.degrees(-10));
+ this.lastServoEnabledAngle_[node.servoPort] = Angle.toRadiansValue(Angle.degrees(-1));
break;
}
case Node.Type.TouchSensor: {
@@ -939,25 +667,10 @@ class RobotBinding {
this.robot_ = null;
}
+}
+
- set realisticSensors(realisticSensors: boolean) {
- for (const digitalSensor of Object.values(this.digitalSensors_)) {
- digitalSensor.realistic = realisticSensors;
- }
- for (const analogSensor of Object.values(this.analogSensors_)) {
- analogSensor.realistic = realisticSensors;
- }
- }
- set noisySensors(noisySensors: boolean) {
- for (const digitalSensor of Object.values(this.digitalSensors_)) {
- digitalSensor.noisy = noisySensors;
- }
- for (const analogSensor of Object.values(this.analogSensors_)) {
- analogSensor.noisy = noisySensors;
- }
- }
-}
namespace RobotBinding {
export interface TickOut {
diff --git a/src/Bindings/RobotLink.ts b/src/Bindings/RobotLink.ts
new file mode 100644
index 00000000..e07b9f1b
--- /dev/null
+++ b/src/Bindings/RobotLink.ts
@@ -0,0 +1,160 @@
+
+import { Scene as babylScene, Vector3, Mesh, SceneLoader,
+ PhysicsBody, PhysicsMotionType, PhysicsShape,
+ PhysicsAggregate, PhysicsShapeType, PhysicShapeOptions,
+ PhysicsShapeParameters, PhysicsShapeContainer
+} from '@babylonjs/core';
+
+import Geometry from '../state/State/Robot/Geometry';
+import Node from '../state/State/Robot/Node';
+import { RENDER_SCALE_METERS_MULTIPLIER } from '../components/Constants/renderConstants';
+import Robot from '../state/State/Robot';
+import { Mass } from '../util/Value';
+
+
+interface BuiltGeometry {
+ nonColliders: Mesh[];
+ colliders?: BuiltGeometry.Collider[];
+}
+
+namespace BuiltGeometry {
+ export interface Collider {
+ name: string;
+ mesh: Mesh;
+ type: number;
+ volume: number;
+ }
+}
+
+// Loads the geometry of a robot part and divides into the collider and noncollider pieces
+export const buildGeometry_ = async (name: string, geometry: Geometry, bScene_: babylScene): Promise<BuiltGeometry> => {
+ let ret: BuiltGeometry;
+ switch (geometry.type) {
+ case 'remote-mesh': {
+ const index = geometry.uri.lastIndexOf('/');
+ const fileName = geometry.uri.substring(index + 1);
+ const baseName = geometry.uri.substring(0, index + 1);
+
+ const res = await SceneLoader.ImportMeshAsync(geometry.include ?? '', baseName, fileName, bScene_);
+
+ const nonColliders: Mesh[] = [];
+ const colliders: BuiltGeometry.Collider[] = [];
+ for (const mesh of res.meshes.slice(1) as Mesh[]) {
+ // The robot mesh includes sub-meshes with the 'collider' name to indicate their use.
+ if (mesh.name.startsWith('collider')) {
+ const parts = mesh.name.split('-');
+ if (parts.length !== 3) throw new Error(`Invalid collider name: ${mesh.name}`);
+ const [_, type, name] = parts;
+ const { extendSize } = mesh.getBoundingInfo().boundingBox;
+ const volume = extendSize.x * extendSize.y * extendSize.z;
+ let bType: number;
+ switch (type) {
+ case 'box': bType = PhysicsShapeType.BOX; break;
+ case 'sphere': bType = PhysicsShapeType.SPHERE; break;
+ case 'cylinder': bType = PhysicsShapeType.CYLINDER; break;
+ case 'capsule': bType = PhysicsShapeType.CAPSULE; break;
+ case 'plane': bType = PhysicsShapeType.HEIGHTFIELD; break;
+ case 'mesh': bType = PhysicsShapeType.MESH; break;
+ default: throw new Error(`Invalid collider type: ${type}`);
+ }
+ colliders.push({ mesh, type: bType, name, volume });
+ } else {
+ nonColliders.push(mesh);
+ }
+ }
+ ret = { nonColliders, colliders };
+ break;
+ }
+ default: { throw new Error(`Unsupported geometry type: ${geometry.type}`); }
+ }
+ return ret;
+};
+
+
+
+// Creates a link and returns the root mesh. Links are wheels, chasis, arms, etc.
+// Loads the geometry and adds the appropriate physics properties to the mesh
+export const createLink_ = async (id: string, link: Node.Link, bScene_: babylScene, robot_: Robot, colliders_: Set<Mesh>) => {
+ let builtGeometry: BuiltGeometry;
+ if (link.geometryId === undefined) {
+ builtGeometry = { nonColliders: [new Mesh(id, bScene_)] };
+ } else {
+ const geometry = robot_.geometry[link.geometryId];
+ if (!geometry) throw new Error(`Missing geometry: ${link.geometryId}`);
+ builtGeometry = await buildGeometry_(id, geometry, bScene_);
+ }
+
+ const meshes = builtGeometry.nonColliders;
+ let myMesh: Mesh;
+ const inertiaScale = 1;
+
+ switch (link.collisionBody.type) {
+ // Notes on Links - the root link should have the highest mass and inertia and it should
+ // scale down further out the tree to prevent wild oscillations.
+ // body.setMassProperties can also help setting the inertia vector,
+ // body.setAngularDamping and body.setLinearDamping can help with oscillations
+ case Node.Link.CollisionBody.Type.Box: {
+ // alert("box collision body"); // Currently there are no box collision bodies in the robot model
+ myMesh = Mesh.MergeMeshes(meshes, true, true, undefined, false, true);
+ myMesh.scaling.scaleInPlace(RENDER_SCALE_METERS_MULTIPLIER);
+ const aggregate = new PhysicsAggregate(myMesh, PhysicsShapeType.BOX, {
+ mass: Mass.toGramsValue(link.mass || Mass.grams(10)),
+ friction: link.friction ?? 0.5,
+ restitution: link.restitution ?? 0,
+ startAsleep: true,
+ }, bScene_);
+ colliders_.add(myMesh);
+ break;
+ }
+ case Node.Link.CollisionBody.Type.Cylinder: {
+ myMesh = Mesh.MergeMeshes(meshes, true, true, undefined, false, true);
+ myMesh.scaling.scaleInPlace(RENDER_SCALE_METERS_MULTIPLIER);
+ const aggregate = new PhysicsAggregate(myMesh, PhysicsShapeType.CYLINDER, {
+ mass: Mass.toGramsValue(link.mass || Mass.grams(10)),
+ friction: link.friction ?? 0.5,
+ restitution: link.restitution ?? 0,
+ startAsleep: true,
+ }, bScene_);
+ colliders_.add(myMesh);
+ break;
+ }
+ case Node.Link.CollisionBody.Type.Embedded: {
+ myMesh = Mesh.MergeMeshes(meshes, true, true, undefined, false, true);
+ myMesh.scaling.scaleInPlace(RENDER_SCALE_METERS_MULTIPLIER);
+ // As the embedded collision body consists of multiple meshes, we need to create a parent
+ // This mmeans we are unable to use the physics aggregate
+ const parentShape = new PhysicsShapeContainer(bScene_);
+ for (const collider of builtGeometry.colliders ?? []) {
+ const bCollider = collider.mesh;
+ bCollider.parent = myMesh;
+ const parameters: PhysicsShapeParameters = { mesh: bCollider };
+ const options: PhysicShapeOptions = { type: PhysicsShapeType.MESH, parameters: parameters };
+ const shape = new PhysicsShape(options, bScene_);
+ shape.material = {
+ friction: link.friction ?? 0.5,
+ restitution: link.restitution ?? 0.1,
+ };
+ parentShape.addChild(shape, bCollider.absolutePosition, bCollider.absoluteRotationQuaternion);
+ bCollider.visibility = 0;
+ colliders_.add(bCollider);
+ }
+
+ const body = new PhysicsBody(myMesh, PhysicsMotionType.DYNAMIC, false, bScene_);
+ body.shape = parentShape;
+ if (link.inertia) {
+ body.setMassProperties({
+ mass: Mass.toGramsValue(link.mass),
+ inertia: new Vector3(link.inertia[0], link.inertia[1], link.inertia[2]) // (left/right, twist around, rock forward and backward)
+ });
+ }
+ body.setAngularDamping(.5);
+
+ colliders_.add(myMesh);
+ break;
+ }
+ default: {
+ throw new Error(`Unsupported collision body type: ${link.collisionBody.type}`);
+ }
+ }
+ return myMesh;
+};
\ No newline at end of file
diff --git a/src/SceneBinding.ts b/src/Bindings/SceneBinding.ts
similarity index 63%
rename from src/SceneBinding.ts
rename to src/Bindings/SceneBinding.ts
index edfd587c..4cc03317 100644
--- a/src/SceneBinding.ts
+++ b/src/Bindings/SceneBinding.ts
@@ -10,28 +10,34 @@ import { PhysicsShapeType, IPhysicsCollisionEvent, IPhysicsEnginePluginV2, Physi
import '@babylonjs/core/Engines/Extensions/engine.views';
import '@babylonjs/core/Lights/Shadows/shadowGeneratorSceneComponent';
-import Dict from "./util/Dict";
-import { RawQuaternion, RawVector2, RawVector3 } from "./util/math";
-import Scene from "./state/State/Scene";
-import Camera from "./state/State/Scene/Camera";
-import Geometry from "./state/State/Scene/Geometry";
-import Node from "./state/State/Scene/Node";
-import Patch from "./util/Patch";
-
-import { ReferenceFramewUnits, RotationwUnits, Vector3wUnits } from "./util/unit-math";
-import { Angle, Distance, Mass, SetOps } from "./util";
-import { Color } from './state/State/Scene/Color';
-import Material from './state/State/Scene/Material';
-import { preBuiltGeometries, preBuiltTemplates } from "./NodeTemplates";
+import Dict from "../util/Dict";
+import { RawQuaternion, RawVector2, RawVector3 } from "../util/math";
+import Scene from "../state/State/Scene";
+import Camera from "../state/State/Scene/Camera";
+import Geometry from "../state/State/Scene/Geometry";
+import Node from "../state/State/Scene/Node";
+import Patch from "../util/Patch";
+
+import { ReferenceFramewUnits, RotationwUnits, Vector3wUnits } from "../util/unit-math";
+import { Angle, Distance, Mass, SetOps } from "../util";
+import { Color } from '../state/State/Scene/Color';
+import Material from '../state/State/Scene/Material';
+import { preBuiltGeometries, preBuiltTemplates } from "../NodeTemplates";
import RobotBinding from './RobotBinding';
-import Robot from './state/State/Robot';
-import AbstractRobot from './AbstractRobot';
-import WorkerInstance from "./programming/WorkerInstance";
-import LocalizedString from './util/LocalizedString';
-import ScriptManager from './ScriptManager';
-import { RENDER_SCALE } from './components/Constants/renderConstants';
+import Robot from '../state/State/Robot';
+import AbstractRobot from '../AbstractRobot';
+import WorkerInstance from "../programming/WorkerInstance";
+import LocalizedString from '../util/LocalizedString';
+import ScriptManager from '../ScriptManager';
+import { RENDER_SCALE } from '../components/Constants/renderConstants';
import { number } from 'prop-types';
+import { createMaterial, updateMaterialBasic, updateMaterialPbr } from './MaterialBinding';
+import { createDirectionalLight, createPointLight, createSpotLight } from './Lightbinding';
+import { createCamera } from './CameraBinding';
+import { createObject, createEmpty } from './ObjectBinding';
+import { apply } from './helpers';
+
export type FrameLike = TransformNode | AbstractMesh;
export interface SceneMeshMetadata {
@@ -92,8 +98,6 @@ class SceneBinding {
private materialIdIter_ = 0;
- private seed_ = 0;
-
constructor(bScene: babylScene, physics: IPhysicsEnginePluginV2) {
this.bScene_ = bScene;
this.scene_ = Scene.EMPTY;
@@ -111,7 +115,7 @@ class SceneBinding {
this.root_ = new TransformNode('__scene_root__', this.bScene_);
this.gizmoManager_ = new GizmoManager(this.bScene_);
- this.camera_ = this.createNoneCamera_(Camera.NONE);
+ this.camera_ = createCamera(Camera.NONE, this.bScene_);
// Gizmos are the little arrows that appear when you select an object
this.gizmoManager_.positionGizmoEnabled = true;
@@ -140,408 +144,13 @@ class SceneBinding {
return ret;
}
- // apply_ is used to propogate the function f(m) on children of the specified mesh g
- private static apply_ = (g: babylNode, f: (m: AbstractMesh) => void) => {
- if (g instanceof AbstractMesh) {
- f(g);
- } else {
- (g.getChildren(c => c instanceof AbstractMesh) as AbstractMesh[]).forEach(f);
- }
- };
-
- private random = (max: number, min: number) => {
- let x = Math.sin(this.seed_++) * 10000;
- x = x - Math.floor(x);
- x = ((x - .5) * (max - min)) + ((max + min) / 2);
- return x;
- };
-
- private buildGeometry_ = async (name: string, geometry: Geometry, faceUvs?: RawVector2[]): Promise<FrameLike> => {
- let ret: FrameLike;
- switch (geometry.type) {
- case 'box': {
- const rect = CreateBox(name, {
- updatable:true,
- width: Distance.toCentimetersValue(geometry.size.x),
- height: Distance.toCentimetersValue(geometry.size.y),
- depth: Distance.toCentimetersValue(geometry.size.z),
- faceUV: this.buildGeometryFaceUvs_(faceUvs, 12),
- }, this.bScene_);
- const verts = rect.getVerticesData("position");
- ret = rect;
- break;
- }
- case 'sphere': {
- const bFaceUvs = this.buildGeometryFaceUvs_(faceUvs, 2)?.[0];
- const segments = 4;
- const rock = CreateSphere(name, {
- segments: segments,
- updatable:true,
- frontUVs: bFaceUvs,
- sideOrientation: bFaceUvs ? Mesh.DOUBLESIDE : undefined,
- diameterX:Distance.toCentimetersValue(geometry.radius) * 2,
- diameterY:Distance.toCentimetersValue(geometry.radius) * 2 * geometry.squash,
- diameterZ:Distance.toCentimetersValue(geometry.radius) * 2 * geometry.stretch,
- }, this.bScene_);
-
- const positions = rock.getVerticesData("position");
- // TODO: Replace with custom rocks from blender
- if (name.includes('Rock')) {
- const skip = [25,26,38,39,51,52,64,65];
- for (let i = 14; i < 65; i++) {
- if (skip.includes(i)) {
- continue;
- } else {
- positions[3 * i] = positions[3 * i] + this.random(geometry.noise, -1 * geometry.noise);
- positions[1 + 3 * i] = positions[1 + 3 * i] + this.random(geometry.noise, -1 * geometry.noise);
- positions[2 + 3 * i] = positions[2 + 3 * i] + this.random(geometry.noise, -1 * geometry.noise);
- }
- }
- }
- rock.updateVerticesData("position", positions);
-
- ret = rock;
- break;
- }
- case 'cylinder': {
- ret = CreateCylinder(name, {
- height: Distance.toCentimetersValue(geometry.height),
- diameterTop: Distance.toCentimetersValue(geometry.radius) * 2,
- diameterBottom: Distance.toCentimetersValue(geometry.radius) * 2,
- faceUV: this.buildGeometryFaceUvs_(faceUvs, 6),
- }, this.bScene_);
- break;
- }
- case 'cone': {
- ret = CreateCylinder(name, {
- diameterTop: 0,
- height: Distance.toCentimetersValue(geometry.height),
- diameterBottom: Distance.toCentimetersValue(geometry.radius) * 2,
- faceUV: this.buildGeometryFaceUvs_(faceUvs, 6),
- }, this.bScene_);
- break;
- }
- case 'plane': {
- ret = CreatePlane(name, {
- width: Distance.toCentimetersValue(geometry.size.x),
- height: Distance.toCentimetersValue(geometry.size.y),
- frontUVs: this.buildGeometryFaceUvs_(faceUvs, 2)?.[0],
- }, this.bScene_);
- break;
- }
- case 'file': {
- const index = geometry.uri.lastIndexOf('/');
- const fileName = geometry.uri.substring(index + 1);
- const baseName = geometry.uri.substring(0, index + 1);
-
- const res = await SceneLoader.ImportMeshAsync(geometry.include ?? '', baseName, fileName, this.bScene_);
- if (res.meshes.length === 1) return res.meshes[0];
- // const nonColliders: Mesh[] = [];
-
- ret = new TransformNode(geometry.uri, this.bScene_);
- for (const mesh of res.meshes as Mesh[]) {
- // GLTF importer adds a __root__ mesh (always the first one) that we can ignore
- if (mesh.name === '__root__') continue;
- // nonColliders.push(mesh);
-
- mesh.setParent(ret);
- }
- // const mesh = Mesh.MergeMeshes(nonColliders, true, true, undefined, false, true);
- break;
- }
- default: {
- throw new Error(`Unsupported geometry type: ${geometry.type}`);
- }
- }
-
- if (ret instanceof AbstractMesh) {
- ret.visibility = 1;
- } else {
- const children = ret.getChildren(c => c instanceof AbstractMesh) as Mesh[];
- const mesh = Mesh.MergeMeshes(children, true, true, undefined, false, true);
- mesh.visibility = 1;
- ret = mesh;
- }
-
- return ret;
- };
-
- private buildGeometryFaceUvs_ = (faceUvs: RawVector2[] | undefined, expectedUvs: number): Vector4[] => {
- if (faceUvs?.length !== expectedUvs) {
- return undefined;
- }
-
- const ret: Vector4[] = [];
- for (let i = 0; i + 1 < faceUvs.length; i += 2) {
- ret.push(new Vector4(faceUvs[i].x, faceUvs[i].y, faceUvs[i + 1].x, faceUvs[i + 1].y));
- }
-
- return ret;
- };
-
private findBNode_ = (id?: string, defaultToRoot?: boolean): babylNode => {
if (id === undefined && defaultToRoot) return this.root_;
if (id !== undefined && !(id in this.nodes_)) throw new Error(`${id} doesn't exist`);
return this.nodes_[id];
};
- private createMaterial_ = (id: string, material: Material) => {
-
- let bMaterial: babylMaterial;
- switch (material.type) {
- case 'basic': {
- const basic = new StandardMaterial(id, this.bScene_);
- const { color } = material;
-
- if (color) {
- switch (color.type) {
- case 'color3': {
- basic.diffuseColor = Color.toBabylon(color.color);
- basic.diffuseTexture = null;
- break;
- }
- case 'texture': {
- if (!color.uri) {
- basic.diffuseColor = new Color3(0.5, 0, 0.5);
- } else {
- if (id.includes('Sky')) {
- basic.reflectionTexture = new Texture(color.uri, this.bScene_);
- basic.reflectionTexture.coordinatesMode = Texture.FIXED_EQUIRECTANGULAR_MODE;
- basic.backFaceCulling = false;
- basic.disableLighting = true;
- } else if (id === 'Container') {
- const myDynamicTexture = new DynamicTexture("dynamic texture", 1000, this.bScene_, true);
- // myDynamicTexture.drawText(material.text, 130, 600, "18px Arial", "white", "gray", true);
- myDynamicTexture.drawText(color.uri, 130, 600, "18px Arial", "white", "gray", true);
- basic.diffuseTexture = myDynamicTexture;
- } else {
- basic.bumpTexture = new Texture(color.uri, this.bScene_, false, false);
- basic.emissiveTexture = new Texture(color.uri, this.bScene_, false, false);
- basic.diffuseTexture = new Texture(color.uri, this.bScene_, false, false);
- basic.diffuseTexture.coordinatesMode = Texture.FIXED_EQUIRECTANGULAR_MODE;
- basic.backFaceCulling = false;
- }
- }
- break;
- }
- }
- }
- bMaterial = basic;
- break;
- }
- case 'pbr': {
- const pbr = new PBRMaterial(id, this.bScene_);
- const { albedo, ambient, emissive, metalness, reflection } = material;
- if (albedo) {
- switch (albedo.type) {
- case 'color3': {
- pbr.albedoColor = Color.toBabylon(albedo.color);
- break;
- }
- case 'texture': {
- pbr.albedoTexture = new Texture(albedo.uri, this.bScene_);
- break;
- }
- }
- }
-
- if (ambient) {
- switch (ambient.type) {
- case 'color3': {
- pbr.ambientColor = Color.toBabylon(ambient.color);
- break;
- }
- case 'texture': {
- pbr.ambientTexture = new Texture(ambient.uri, this.bScene_);
- break;
- }
- }
- }
-
- if (emissive) {
- const glow = new GlowLayer('glow', this.bScene_);
- switch (emissive.type) {
- case 'color3': {
- pbr.emissiveColor = Color.toBabylon(emissive.color);
- break;
- }
- case 'texture': {
- pbr.emissiveTexture = new Texture(emissive.uri, this.bScene_);
- break;
- }
- }
- }
-
- if (metalness) {
- switch (metalness.type) {
- case 'color1': {
- pbr.metallic = metalness.color;
- break;
- }
- case 'texture': {
- pbr.metallicTexture = new Texture(metalness.uri, this.bScene_);
- break;
- }
- }
- }
-
- if (reflection) {
- switch (reflection.type) {
- case 'color3': {
- pbr.reflectivityColor = Color.toBabylon(reflection.color);
- break;
- }
- case 'texture': {
- pbr.reflectivityTexture = new Texture(reflection.uri, this.bScene_);
- break;
- }
- }
- }
-
- bMaterial = pbr;
-
- break;
- }
- }
-
- return bMaterial;
- };
-
- private updateMaterialBasic_ = (bMaterial: StandardMaterial, material: Patch.InnerPatch<Material.Basic>) => {
- const { color } = material;
-
- if (color.type === Patch.Type.InnerChange || color.type === Patch.Type.OuterChange) {
- switch (color.next.type) {
- case 'color3': {
-
- bMaterial.diffuseColor = Color.toBabylon(color.next.color);
- bMaterial.diffuseTexture = null;
- break;
- }
- case 'texture': {
- if (!color.next.uri) {
- bMaterial.diffuseColor = new Color3(0.5, 0, 0.5);
- bMaterial.diffuseTexture = null;
- } else if (color.next.uri[0] !== '/') {
- const myDynamicTexture = new DynamicTexture("dynamic texture", 1000, this.bScene_, true);
- // myDynamicTexture.drawText(material.text, 130, 600, "18px Arial", "white", "gray", true);
- myDynamicTexture.drawText(color.next.uri, 130, 600, "18px Arial", "white", "gray", true);
- bMaterial.diffuseTexture = myDynamicTexture;
- } else {
- bMaterial.diffuseColor = Color.toBabylon(Color.WHITE);
- bMaterial.diffuseTexture = new Texture(color.next.uri, this.bScene_);
- }
- break;
- }
- }
- }
-
- return bMaterial;
- };
-
- private updateMaterialPbr_ = (bMaterial: PBRMaterial, material: Patch.InnerPatch<Material.Pbr>) => {
- const { albedo, ambient, emissive, metalness, reflection } = material;
-
- if (albedo.type === Patch.Type.OuterChange) {
- switch (albedo.next.type) {
- case 'color3': {
- bMaterial.albedoColor = Color.toBabylon(albedo.next.color);
- bMaterial.albedoTexture = null;
- break;
- }
- case 'texture': {
- if (!albedo.next.uri) {
- bMaterial.albedoColor = new Color3(0.5, 0, 0.5);
- } else {
- bMaterial.albedoColor = Color.toBabylon(Color.WHITE);
- bMaterial.albedoTexture = new Texture(albedo.next.uri, this.bScene_);
- }
- break;
- }
- }
- }
-
- if (ambient.type === Patch.Type.OuterChange) {
- switch (ambient.next.type) {
- case 'color3': {
- bMaterial.ambientColor = Color.toBabylon(ambient.next.color);
- bMaterial.ambientTexture = null;
- break;
- }
- case 'texture': {
- if (!ambient.next.uri) {
- bMaterial.ambientColor = new Color3(0.5, 0, 0.5);
- bMaterial.ambientTexture = null;
- } else {
- bMaterial.ambientColor = Color.toBabylon(Color.WHITE);
- bMaterial.ambientTexture = new Texture(ambient.next.uri, this.bScene_);
- }
- break;
- }
- }
- }
-
- if (emissive.type === Patch.Type.OuterChange) {
- switch (emissive.next.type) {
- case 'color3': {
- bMaterial.emissiveColor = Color.toBabylon(emissive.next.color);
- bMaterial.emissiveTexture = null;
- break;
- }
- case 'texture': {
- if (!emissive.next.uri) {
- bMaterial.emissiveColor = new Color3(0.5, 0, 0.5);
- bMaterial.emissiveTexture = null;
- } else {
- bMaterial.emissiveColor = Color.toBabylon(Color.BLACK);
- bMaterial.emissiveTexture = new Texture(emissive.next.uri, this.bScene_);
- }
- break;
- }
- }
- }
-
- if (metalness.type === Patch.Type.OuterChange) {
- switch (metalness.next.type) {
- case 'color1': {
- bMaterial.metallic = metalness.next.color;
- bMaterial.metallicTexture = null;
- break;
- }
- case 'texture': {
- if (!metalness.next.uri) {
- bMaterial.metallic = 0;
- } else {
- bMaterial.metallicTexture = new Texture(metalness.next.uri, this.bScene_);
- }
- break;
- }
- }
- }
-
- if (reflection.type === Patch.Type.OuterChange) {
- switch (reflection.next.type) {
- case 'color3': {
- bMaterial.reflectivityColor = Color.toBabylon(reflection.next.color);
- bMaterial.reflectivityTexture = null;
- break;
- }
- case 'texture': {
- if (!reflection.next.uri) {
- bMaterial.reflectivityColor = new Color3(0.5, 0, 0.5);
- bMaterial.reflectivityTexture = null;
- } else {
- bMaterial.reflectivityColor = Color.toBabylon(Color.WHITE);
- bMaterial.reflectivityTexture = new Texture(reflection.next.uri, this.bScene_);
- }
- break;
- }
- }
- }
-
- return bMaterial;
- };
-
+
private updateMaterial_ = (bMaterial: babylMaterial, material: Patch<Material>) => {
switch (material.type) {
case Patch.Type.OuterChange: {
@@ -549,7 +158,7 @@ class SceneBinding {
const id = bMaterial ? `${bMaterial.id}` : `Scene Material ${this.materialIdIter_++}`;
if (bMaterial) bMaterial.dispose();
if (next) {
- return this.createMaterial_(id, next);
+ return createMaterial(id, next, this.bScene_);
}
return null;
}
@@ -557,10 +166,10 @@ class SceneBinding {
const { inner, next } = material;
switch (next.type) {
case 'basic': {
- return this.updateMaterialBasic_(bMaterial as StandardMaterial, inner as Patch.InnerPatch<Material.Basic>);
+ return updateMaterialBasic(bMaterial as StandardMaterial, inner as Patch.InnerPatch<Material.Basic>, this.bScene_);
}
case 'pbr': {
- return this.updateMaterialPbr_(bMaterial as PBRMaterial, inner as Patch.InnerPatch<Material.Pbr>);
+ return updateMaterialPbr(bMaterial as PBRMaterial, inner as Patch.InnerPatch<Material.Pbr>, this.bScene_);
}
}
break;
@@ -570,86 +179,6 @@ class SceneBinding {
return bMaterial;
};
- private createObject_ = async (node: Node.Obj, nextScene: Scene): Promise<babylNode> => {
- const parent = this.findBNode_(node.parentId, true);
-
- const geometry = nextScene.geometry[node.geometryId] ?? preBuiltGeometries[node.geometryId];
- if (!geometry) {
- console.error(`node ${LocalizedString.lookup(node.name, LocalizedString.EN_US)} has invalid geometry ID: ${node.geometryId}`);
- return null;
- }
- const ret = await this.buildGeometry_(node.name[LocalizedString.EN_US], geometry, node.faceUvs);
-
- if (!node.visible) {
- SceneBinding.apply_(ret, m => m.isVisible = false);
- }
-
- if (node.material) {
- const material = this.createMaterial_(node.name[LocalizedString.EN_US], node.material);
- SceneBinding.apply_(ret, m => m.material = material);
- }
-
- ret.setParent(parent);
- return ret;
- };
-
- private createEmpty_ = (node: Node.Empty): TransformNode => {
- const parent = this.findBNode_(node.parentId, true);
-
- const ret = new TransformNode(node.name[LocalizedString.EN_US], this.bScene_);
- ret.setParent(parent);
- return ret;
- };
-
- private createDirectionalLight_ = (id: string, node: Node.DirectionalLight): DirectionalLight => {
- const ret = new DirectionalLight(node.name[LocalizedString.EN_US], RawVector3.toBabylon(node.direction), this.bScene_);
-
- ret.intensity = node.intensity;
- if (node.radius !== undefined) ret.radius = node.radius;
- if (node.range !== undefined) ret.range = node.range;
-
- this.shadowGenerators_[id] = SceneBinding.createShadowGenerator_(ret);
-
- return ret;
- };
-
- private createSpotLight_ = (id: string, node: Node.SpotLight): SpotLight => {
- const origin: ReferenceFramewUnits = node.origin ?? {};
- const position: Vector3wUnits = origin.position ?? Vector3wUnits.zero();
-
- const ret = new SpotLight(
- node.name[LocalizedString.EN_US],
- RawVector3.toBabylon(Vector3wUnits.toRaw(position, 'centimeters')),
- RawVector3.toBabylon(node.direction),
- Angle.toRadiansValue(node.angle),
- node.exponent,
- this.bScene_
- );
-
- this.shadowGenerators_[id] = SceneBinding.createShadowGenerator_(ret);
-
- return ret;
- };
-
- private createPointLight_ = (id: string, node: Node.PointLight): PointLight => {
- const origin: ReferenceFramewUnits = node.origin ?? {};
- const position: Vector3wUnits = origin.position ?? Vector3wUnits.zero();
-
- const ret = new PointLight(
- node.name[LocalizedString.EN_US],
- RawVector3.toBabylon(Vector3wUnits.toRaw(position, 'centimeters')),
- this.bScene_
- );
-
- ret.intensity = node.intensity;
-
- this.shadowGenerators_[id] = SceneBinding.createShadowGenerator_(ret);
-
- ret.setEnabled(node.visible);
-
- return ret;
- };
-
// Create Robot Binding
private createRobot_ = async (id: string, node: Node.Robot): Promise<RobotBinding> => {
// This should probably be somewhere else, but it ensures this is called during
@@ -657,12 +186,11 @@ class SceneBinding {
WorkerInstance.sync(node.state);
const robotBinding = new RobotBinding(this.bScene_, this.physicsViewer_);
+
const robot = this.robots_[node.robotId];
if (!robot) throw new Error(`Robot by id "${node.robotId}" not found`);
await robotBinding.setRobot(node, robot, id);
robotBinding.linkOrigins = this.robotLinkOrigins_[id] || {};
- // console.log('robot linkOrigins', robotBinding.linkOrigins);
- // Here the linkOrigins are all shown as 0,0,0, this may be why the initial kinematics are messed up.
robotBinding.visible = true;
const observerObj: { observer: Observer<babylScene> } = { observer: null };
@@ -695,14 +223,6 @@ class SceneBinding {
return robotBinding;
};
- private static createShadowGenerator_ = (light: IShadowLight) => {
- const ret = new ShadowGenerator(1024, light);
- ret.useKernelBlur = false;
- ret.blurScale = 2;
- ret.filter = ShadowGenerator.FILTER_POISSONSAMPLING;
- return ret;
- };
-
private createNode_ = async (id: string, node: Node, nextScene: Scene): Promise<babylNode> => {
let nodeToCreate: Node = node;
@@ -724,11 +244,17 @@ class SceneBinding {
let ret: babylNode;
switch (nodeToCreate.type) {
- case 'object': ret = await this.createObject_(nodeToCreate, nextScene); break;
- case 'empty': ret = this.createEmpty_(nodeToCreate); break;
- case 'directional-light': ret = this.createDirectionalLight_(id, nodeToCreate); break;
- case 'spot-light': ret = this.createSpotLight_(id, nodeToCreate); break;
- case 'point-light': ret = this.createPointLight_(id, nodeToCreate); break;
+ case 'object': {
+ const parent = this.findBNode_(nodeToCreate.parentId, true);
+ ret = await createObject(nodeToCreate, nextScene, parent, this.bScene_); break;
+ }
+ case 'empty': {
+ const parent = this.findBNode_(nodeToCreate.parentId, true);
+ ret = createEmpty(nodeToCreate, parent, this.bScene_); break;
+ }
+ case 'directional-light': ret = createDirectionalLight(id, nodeToCreate, this.bScene_, this.shadowGenerators_); break;
+ case 'spot-light': ret = createSpotLight(id, nodeToCreate, this.bScene_, this.shadowGenerators_); break;
+ case 'point-light': ret = createPointLight(id, nodeToCreate, this.bScene_, this.shadowGenerators_); break;
case 'robot': await this.createRobot_(id, nodeToCreate); break;
default: {
console.warn('invalid node type for create node:', nodeToCreate.type);
@@ -744,7 +270,7 @@ class SceneBinding {
ret.metadata = { id } as SceneMeshMetadata;
if (ret instanceof AbstractMesh || ret instanceof TransformNode) {
- SceneBinding.apply_(ret, m => {
+ apply(ret, m => {
m.metadata = { id } as SceneMeshMetadata;
this.restorePhysicsToObject(m, nodeToCreate as Node.Obj, null, nextScene);
});
@@ -824,7 +350,7 @@ class SceneBinding {
let bMaterial = this.findMaterial_(bNode);
bMaterial = this.updateMaterial_(bMaterial, node.inner.material);
- SceneBinding.apply_(bNode, m => {
+ apply(bNode, m => {
m.material = bMaterial;
});
@@ -834,7 +360,7 @@ class SceneBinding {
}
if (node.inner.physics.type === Patch.Type.OuterChange) {
- SceneBinding.apply_(bNode, m => {
+ apply(bNode, m => {
this.removePhysicsFromObject(m);
this.restorePhysicsToObject(m, node.next, id, nextScene);
});
@@ -842,7 +368,7 @@ class SceneBinding {
if (node.inner.visible.type === Patch.Type.OuterChange) {
const nextVisible = node.inner.visible.next;
- SceneBinding.apply_(bNode, m => {
+ apply(bNode, m => {
m.isVisible = nextVisible;
// Create/remove physics for object becoming visible/invisible
@@ -1093,29 +619,6 @@ class SceneBinding {
private gizmoManager_: GizmoManager;
- private createArcRotateCamera_ = (camera: Camera.ArcRotate): ArcRotateCamera => {
- const ret = new ArcRotateCamera('botcam', 0, 0, 0, Vector3wUnits.toBabylon(camera.target, 'centimeters'), this.bScene_);
- ret.attachControl(this.bScene_.getEngine().getRenderingCanvas(), true);
- ret.position = Vector3wUnits.toBabylon(camera.position, 'centimeters');
- ret.panningSensibility = 100;
- // ret.checkCollisions = true;
-
- return ret;
- };
-
- private createNoneCamera_ = (camera: Camera.None): ArcRotateCamera => {
- const ret = new ArcRotateCamera('botcam', 10, 10, 10, Vector3wUnits.toBabylon(Vector3wUnits.zero(), 'centimeters'), this.bScene_);
- ret.attachControl(this.bScene_.getEngine().getRenderingCanvas(), true);
-
- return ret;
- };
-
- private createCamera_ = (camera: Camera): babylCamera => {
- switch (camera.type) {
- case 'arc-rotate': return this.createArcRotateCamera_(camera);
- case 'none': return this.createNoneCamera_(camera);
- }
- };
private updateArcRotateCamera_ = (node: Patch.InnerChange<Camera.ArcRotate>): ArcRotateCamera => {
if (!(this.camera_ instanceof ArcRotateCamera)) throw new Error('Expected ArcRotateCamera');
@@ -1315,7 +818,7 @@ class SceneBinding {
const prevBNode = this.bScene_.getNodeById(prev);
if (prevNodeObj && (prevBNode instanceof AbstractMesh || prevBNode instanceof TransformNode)) {
prevBNode.metadata = { ...(prevBNode.metadata as SceneMeshMetadata), selected: false };
- SceneBinding.apply_(prevBNode, m => this.restorePhysicsToObject(m, prevNodeObj, prev, scene));
+ apply(prevBNode, m => this.restorePhysicsToObject(m, prevNodeObj, prev, scene));
}
this.gizmoManager_.attachToNode(null);
@@ -1325,7 +828,7 @@ class SceneBinding {
if (next !== undefined) {
const node = this.bScene_.getNodeById(next);
if (node instanceof AbstractMesh || node instanceof TransformNode) {
- SceneBinding.apply_(node, m => this.removePhysicsFromObject(m));
+ apply(node, m => this.removePhysicsFromObject(m));
node.metadata = { ...(node.metadata as SceneMeshMetadata), selected: true };
this.gizmoManager_.attachToNode(node);
}
@@ -1335,12 +838,12 @@ class SceneBinding {
const oldCamera = this.camera_;
switch (patch.camera.type) {
case Patch.Type.OuterChange: {
- this.camera_ = this.createCamera_(patch.camera.next);
+ this.camera_ = createCamera(patch.camera.next, this.bScene_);
break;
}
case Patch.Type.InnerChange: {
if (this.camera_) this.camera_ = this.updateCamera_(patch.camera);
- else this.camera_ = this.createCamera_(patch.camera.next);
+ else this.camera_ = createCamera(patch.camera.next, this.bScene_);
break;
}
}
diff --git a/src/Bindings/WeightBinding.ts b/src/Bindings/WeightBinding.ts
new file mode 100644
index 00000000..13faa2fb
--- /dev/null
+++ b/src/Bindings/WeightBinding.ts
@@ -0,0 +1,44 @@
+
+import { Scene as babylScene, CreateSphere, Vector3, Mesh,
+ PhysicsAggregate, PhysicsShapeType, LockConstraint,
+} from '@babylonjs/core';
+
+import Node from '../state/State/Robot/Node';
+import { ReferenceFramewUnits } from '../util/unit-math';
+import { RENDER_SCALE } from '../components/Constants/renderConstants';
+import { Mass } from '../util/Value';
+import Robot from '../state/State/Robot';
+import Dict from '../util/Dict';
+
+// Adds an invisible weight to a parent link.
+export const createWeight_ = (id: string, weight: Node.Weight, bScene_: babylScene, robot_: Robot, links_: Dict<Mesh>) => {
+ const ret = CreateSphere(id, { diameter: 1 }, bScene_);
+ ret.visibility = 0;
+
+ const parent = robot_.nodes[weight.parentId];
+ if (!parent) throw new Error(`Missing parent: "${weight.parentId}" for weight "${id}"`);
+ if (parent.type !== Node.Type.Link) throw new Error(`Invalid parent type: "${parent.type}" for weight "${id}"`);
+
+ const aggregate = new PhysicsAggregate(ret, PhysicsShapeType.CYLINDER, {
+ mass: Mass.toGramsValue(weight.mass),
+ friction: 0,
+ restitution: 0,
+ }, bScene_);
+
+ const bParent = links_[weight.parentId];
+ if (!bParent) throw new Error(`Missing parent instantiation: "${weight.parentId}" for weight "${id}"`);
+
+ const bOrigin = ReferenceFramewUnits.toBabylon(weight.origin, RENDER_SCALE);
+
+ const constraint = new LockConstraint(
+ bOrigin.position,
+ new Vector3(0,0,0), // RawVector3.toBabylon(new Vector3(-8,10,0)), // updown, frontback
+ Vector3.Up(),
+ Vector3.Up().applyRotationQuaternion(bOrigin.rotationQuaternion.invert()),
+ bScene_
+ );
+
+ bParent.physicsBody.addConstraint(ret.physicsBody, constraint);
+
+ return ret;
+};
\ No newline at end of file
diff --git a/src/Bindings/helpers.ts b/src/Bindings/helpers.ts
new file mode 100644
index 00000000..34aa13a8
--- /dev/null
+++ b/src/Bindings/helpers.ts
@@ -0,0 +1,16 @@
+
+import { PhysicsShapeType, IPhysicsCollisionEvent, IPhysicsEnginePluginV2, PhysicsAggregate,
+ TransformNode, AbstractMesh, PhysicsViewer, ShadowGenerator, CreateBox, CreateSphere, CreateCylinder,
+ CreatePlane, Vector4, Vector3, Texture, DynamicTexture, StandardMaterial, GizmoManager, ArcRotateCamera,
+ IShadowLight, PointLight, SpotLight, DirectionalLight, Color3, PBRMaterial, Mesh, SceneLoader, EngineView,
+ Scene as babylScene, Node as babylNode, Camera as babylCamera, Material as babylMaterial,
+ GlowLayer, Observer, BoundingBox } from '@babylonjs/core';
+
+
+export const apply = (g: babylNode, f: (m: AbstractMesh) => void) => {
+ if (g instanceof AbstractMesh) {
+ f(g);
+ } else {
+ (g.getChildren(c => c instanceof AbstractMesh) as AbstractMesh[]).forEach(f);
+ }
+};
\ No newline at end of file
diff --git a/src/Sim.tsx b/src/Sim.tsx
index ae0da088..172816b2 100644
--- a/src/Sim.tsx
+++ b/src/Sim.tsx
@@ -16,7 +16,7 @@ import { Angle } from './util';
import store from './state';
import { Unsubscribe } from 'redux';
import { ScenesAction } from './state/reducer';
-import SceneBinding, { SceneMeshMetadata } from './SceneBinding';
+import SceneBinding, { SceneMeshMetadata } from './Bindings/SceneBinding';
import Scene from './state/State/Scene';
import Node from './state/State/Scene/Node';
import { Robots } from './state/State';
diff --git a/src/robots/demobot.ts b/src/robots/demobot.ts
index 85e1551e..7b8ea70a 100644
--- a/src/robots/demobot.ts
+++ b/src/robots/demobot.ts
@@ -21,6 +21,7 @@ export const DEMOBOT: Robot = {
mass: grams(400),
restitution: 0,
friction: 0.01,
+ inertia: [10, 10, 10]
}),
lightSensor: Node.lightSensor({
parentId: 'chassis',
@@ -83,6 +84,7 @@ export const DEMOBOT: Robot = {
mass: grams(14),
friction: 50,
restitution: 0,
+ inertia: [6, 6, 6],
collisionBody: Node.Link.CollisionBody.EMBEDDED,
}),
claw: Node.servo({
@@ -99,6 +101,7 @@ export const DEMOBOT: Robot = {
mass: grams(7),
friction: 50,
restitution: 0,
+ inertia: [3, 3, 3],
collisionBody: Node.Link.CollisionBody.EMBEDDED,
}),
touch_sensor: Node.touchSensor({