diff --git a/src/main/java/com/neuronrobotics/bowlerkernel/Bezier3d/BezierEditor.java b/src/main/java/com/neuronrobotics/bowlerkernel/Bezier3d/BezierEditor.java index 0a9f5a130..00f60f7a9 100644 --- a/src/main/java/com/neuronrobotics/bowlerkernel/Bezier3d/BezierEditor.java +++ b/src/main/java/com/neuronrobotics/bowlerkernel/Bezier3d/BezierEditor.java @@ -238,10 +238,10 @@ private TransformNR updateLines(CartesianManipulator m, CartesianManipulator p, double xyRot = Math.toDegrees(Math.atan2(cp1Ydiff, cp1XDiff)) - 90; TransformNR az = new TransformNR(0, 0, 0, new RotationNR(0, -xyRot, 0)); - TransformNR reorented = az.times(vect); - // com.neuronrobotics.sdk.common.Log.error("CP1 "+reorented.getX()+" "+reorented.getY()+" - // "+reorented.getZ()); - double xzRot = Math.toDegrees(Math.atan2(reorented.getZ(), reorented.getY())); + TransformNR reoriented = az.times(vect); + // com.neuronrobotics.sdk.common.Log.error("CP1 "+reoriented.getX()+" "+reoriented.getY()+" + // "+reoriented.getZ()); + double xzRot = Math.toDegrees(Math.atan2(reoriented.getZ(), reoriented.getY())); TransformNR pose = new TransformNR(0, 0, 0, new RotationNR(xzRot, xyRot, 0)); diff --git a/src/main/java/com/neuronrobotics/bowlerstudio/physics/MobileBasePhysicsManager.java b/src/main/java/com/neuronrobotics/bowlerstudio/physics/MobileBasePhysicsManager.java index a8ec06dd0..a69f0f076 100644 --- a/src/main/java/com/neuronrobotics/bowlerstudio/physics/MobileBasePhysicsManager.java +++ b/src/main/java/com/neuronrobotics/bowlerstudio/physics/MobileBasePhysicsManager.java @@ -52,9 +52,9 @@ private IPhysicsUpdate getUpdater(RigidBody body, IMU base) { Vector3f oldavelocity = new Vector3f(0f, 0f, 0f); Vector3f oldvelocity = new Vector3f(0f, 0f, 0f); Vector3f gravity = new Vector3f(); - private Quat4f orentation = new Quat4f(); + private Quat4f orientation = new Quat4f(); Transform gravTrans = new Transform(); - Transform orentTrans = new Transform(); + Transform orientTrans = new Transform(); Vector3f avelocity = new Vector3f(); Vector3f velocity = new Vector3f(); @@ -65,25 +65,25 @@ public void update(float timeStep) { body.getLinearVelocity(velocity); body.getGravity(gravity); - body.getOrientation(orentation); + body.getOrientation(orientation); TransformFactory.nrToBullet(new TransformNR(gravity.x, gravity.y, gravity.z, new RotationNR()), gravTrans); TransformFactory.nrToBullet( - new TransformNR(0, 0, 0, orentation.w, orentation.x, orentation.y, orentation.z), orentTrans); - orentTrans.inverse(); - orentTrans.mul(gravTrans); + new TransformNR(0, 0, 0, orientation.w, orientation.x, orientation.y, orientation.z), orientTrans); + orientTrans.inverse(); + orientTrans.mul(gravTrans); // A=DeltaV / DeltaT Double rotxAcceleration = (double) ((oldavelocity.x - avelocity.x) / timeStep); Double rotyAcceleration = (double) ((oldavelocity.y - avelocity.y) / timeStep); Double rotzAcceleration = (double) ((oldavelocity.z - avelocity.z) / timeStep); Double xAcceleration = (double) (((oldvelocity.x - velocity.x) / timeStep) / PhysicsGravityScalar) - + (orentTrans.origin.x / PhysicsGravityScalar); + + (orientTrans.origin.x / PhysicsGravityScalar); Double yAcceleration = (double) (((oldvelocity.y - velocity.y) / timeStep) / PhysicsGravityScalar) - + (orentTrans.origin.y / PhysicsGravityScalar); + + (orientTrans.origin.y / PhysicsGravityScalar); Double zAcceleration = (double) (((oldvelocity.z - velocity.z) / timeStep) / PhysicsGravityScalar) - + (orentTrans.origin.z / PhysicsGravityScalar); + + (orientTrans.origin.z / PhysicsGravityScalar); // tell the virtual IMU the system updated base.setVirtualState(new IMUUpdate(xAcceleration, yAcceleration, zAcceleration, rotxAcceleration, rotyAcceleration, rotzAcceleration,base.currentTimeMillis())); diff --git a/src/main/java/com/neuronrobotics/bowlerstudio/scripting/cadoodle/Mirror.java b/src/main/java/com/neuronrobotics/bowlerstudio/scripting/cadoodle/Mirror.java index f66c86a45..01613b05f 100644 --- a/src/main/java/com/neuronrobotics/bowlerstudio/scripting/cadoodle/Mirror.java +++ b/src/main/java/com/neuronrobotics/bowlerstudio/scripting/cadoodle/Mirror.java @@ -16,7 +16,7 @@ public class Mirror extends CaDoodleOperation { @Expose(serialize = true, deserialize = true) - private MirrorOrentation location; + private MirrorOrientation location; @Expose(serialize = true, deserialize = true) private List names = new ArrayList(); @Expose(serialize = true, deserialize = true) @@ -61,13 +61,13 @@ public List process(List incoming) { Transform mirroringCenter = new Transform().movex(base.getCenterX()).movey(base.getCenterY()) .movez(base.getCenterZ()); Transform sc = new Transform(); - if (location == MirrorOrentation.x) { + if (location == MirrorOrientation.x) { sc = new Transform().scaleX(-1); } - if (location == MirrorOrentation.y) { + if (location == MirrorOrientation.y) { sc = new Transform().scaleY(-1); } - if (location == MirrorOrentation.z) { + if (location == MirrorOrientation.z) { sc = new Transform().scaleZ(-1); } Transform scale = sc; @@ -117,13 +117,13 @@ private CSG mirror(CSG csg, String name) { Transform mirroringCenter = new Transform().movex(t.getCenterX()).movex(t.getCenterY()).movez(t.getCenterZ()); CSG centered = t.transformed(mirroringCenter.inverse()); - if (location == MirrorOrentation.x) { + if (location == MirrorOrientation.x) { centered = centered.mirrorx(); } - if (location == MirrorOrentation.y) { + if (location == MirrorOrientation.y) { centered = centered.mirrory(); } - if (location == MirrorOrentation.z) { + if (location == MirrorOrientation.z) { centered = centered.mirrorz(); } centered = centered.transformed(mirroringCenter); @@ -131,11 +131,11 @@ private CSG mirror(CSG csg, String name) { return centered.setName(name).syncProperties(getCaDoodleFile().getCsgDBinstance(), csg); } - public MirrorOrentation getLocation() { + public MirrorOrientation getLocation() { return location; } - public Mirror setLocation(MirrorOrentation location) { + public Mirror setLocation(MirrorOrientation location) { this.location = location; return this; } diff --git a/src/main/java/com/neuronrobotics/bowlerstudio/scripting/cadoodle/MirrorOrentation.java b/src/main/java/com/neuronrobotics/bowlerstudio/scripting/cadoodle/MirrorOrientation.java similarity index 68% rename from src/main/java/com/neuronrobotics/bowlerstudio/scripting/cadoodle/MirrorOrentation.java rename to src/main/java/com/neuronrobotics/bowlerstudio/scripting/cadoodle/MirrorOrientation.java index ee3456c57..894a99559 100644 --- a/src/main/java/com/neuronrobotics/bowlerstudio/scripting/cadoodle/MirrorOrentation.java +++ b/src/main/java/com/neuronrobotics/bowlerstudio/scripting/cadoodle/MirrorOrientation.java @@ -1,5 +1,5 @@ package com.neuronrobotics.bowlerstudio.scripting.cadoodle; -public enum MirrorOrentation { +public enum MirrorOrientation { x,y,z; }