@@ -13,6 +13,7 @@ import { GLTF, GLTFLoader } from "three/examples/jsm/loaders/GLTFLoader.js";
1313import WorkerManager from "../../../../hub/WorkerManager" ;
1414import { AdvantageScopeAssets } from "../../../AdvantageScopeAssets" ;
1515import { SwerveState } from "../../../geometry" ;
16+ import { MechanismState } from "../../../log/LogUtil" ;
1617import { Units } from "../../../units" ;
1718import { transformPx } from "../../../util" ;
1819import { Field3dRendererCommand_GhostObj , Field3dRendererCommand_RobotObj } from "../../Field3dRenderer" ;
@@ -21,6 +22,14 @@ import ObjectManager from "../ObjectManager";
2122import { FTC_MULTIPLIER , XR_MAX_RADIUS } from "../OptimizeGeometries" ;
2223import ResizableInstancedMesh from "../ResizableInstancedMesh" ;
2324
25+ type MechanismLineData = {
26+ mesh : ResizableInstancedMesh ;
27+ geometry : THREE . BoxGeometry ;
28+ scale : THREE . Vector3 ;
29+ translation : THREE . Vector3 ;
30+ material : THREE . MeshPhongMaterial ;
31+ } ;
32+
2433export default class RobotManager extends ObjectManager <
2534 Field3dRendererCommand_RobotObj | Field3dRendererCommand_GhostObj
2635> {
@@ -41,13 +50,8 @@ export default class RobotManager extends ObjectManager<
4150 shininess : this . materialShininess
4251 } ) ;
4352 private visionLines : Line2 [ ] = [ ] ;
44- private mechanismLines : {
45- mesh : ResizableInstancedMesh ;
46- geometry : THREE . BoxGeometry ;
47- scale : THREE . Vector3 ;
48- translation : THREE . Vector3 ;
49- material : THREE . MeshPhongMaterial ;
50- } [ ] = [ ] ;
53+ private mechanismLinesXZ : MechanismLineData [ ] = [ ] ;
54+ private mechanismLinesYZ : MechanismLineData [ ] = [ ] ;
5155
5256 private swerveContainer : HTMLElement | null = null ;
5357 private swerveCanvas : HTMLCanvasElement | null = null ;
@@ -100,9 +104,8 @@ export default class RobotManager extends ObjectManager<
100104 this . meshes . forEach ( ( mesh ) => {
101105 mesh . dispose ( ) ;
102106 } ) ;
103- this . mechanismLines . forEach ( ( entry ) => {
104- entry . mesh . dispose ( ) ;
105- } ) ;
107+ this . mechanismLinesXZ . forEach ( ( entry ) => entry . mesh . dispose ( ) ) ;
108+ this . mechanismLinesYZ . forEach ( ( entry ) => entry . mesh . dispose ( ) ) ;
106109 while ( this . visionLines . length > 0 ) {
107110 this . visionLines [ 0 ] . geometry . dispose ( ) ;
108111 this . visionLines [ 0 ] . material . dispose ( ) ;
@@ -431,95 +434,94 @@ export default class RobotManager extends ObjectManager<
431434 }
432435
433436 // Update mechanism
434- if ( object . mechanism === null ) {
435- // No mechanism data, remove all meshes
436- while ( this . mechanismLines . length > 0 ) {
437- this . mechanismLines [ 0 ] . mesh . dispose ( true , object . type === "robot" ) ; // Ghost material is shared, don't dispose
438- this . mechanismLines . shift ( ) ;
439- }
440- } else {
441- // Filter to visible lines
442- let mechanismLines = object . mechanism ?. lines . filter (
443- ( line ) =>
444- Math . hypot ( line . end [ 1 ] - line . start [ 1 ] , line . end [ 0 ] - line . start [ 0 ] ) >= 1e-3 &&
445- line . weight * this . MECHANISM_WIDTH_PER_WEIGHT >= 1e-3
446- ) ;
447-
448- // Remove extra lines
449- while ( this . mechanismLines . length > mechanismLines . length ) {
450- this . mechanismLines [ 0 ] . mesh . dispose ( true , object . type === "robot" ) ; // Ghost material is shared, don't dispose
451- this . mechanismLines . shift ( ) ;
452- }
453-
454- // Add new lines
455- while ( this . mechanismLines . length < mechanismLines . length ) {
456- const geometry = new THREE . BoxGeometry ( 1 , 1 , 1 ) ;
457- const material =
458- object . type === "ghost"
459- ? this . ghostMaterial
460- : new THREE . MeshPhongMaterial ( { specular : this . materialSpecular , shininess : this . materialShininess } ) ;
461- this . mechanismLines . push ( {
462- mesh : new ResizableInstancedMesh ( this . root , [ { geometry : geometry , material : material } ] ) ,
463- geometry : geometry ,
464- scale : new THREE . Vector3 ( 1 , 1 , 1 ) ,
465- translation : new THREE . Vector3 ( ) ,
466- material : material
467- } ) ;
468- }
469-
470- // Update children
471- for ( let i = 0 ; i < mechanismLines . length ; i ++ ) {
472- const line = mechanismLines [ i ] ;
473- const meshEntry = this . mechanismLines [ i ] ;
474-
475- const length = Math . hypot ( line . end [ 1 ] - line . start [ 1 ] , line . end [ 0 ] - line . start [ 0 ] ) ;
476- const angle = Math . atan2 ( line . end [ 1 ] - line . start [ 1 ] , line . end [ 0 ] - line . start [ 0 ] ) ;
477-
478- // Update length
479- const newScale = new THREE . Vector3 (
480- object . mechanism . axis == "x" ? length : line . weight * this . MECHANISM_WIDTH_PER_WEIGHT ,
481- object . mechanism . axis == "x" ? line . weight * this . MECHANISM_WIDTH_PER_WEIGHT : length ,
482- line . weight * this . MECHANISM_WIDTH_PER_WEIGHT
437+ let updateMechanism = ( state : MechanismState | null , lines : MechanismLineData [ ] , plane : "xz" | "yz" ) => {
438+ if ( state === null ) {
439+ // No mechanism data, remove all meshes
440+ while ( lines . length > 0 ) {
441+ lines [ 0 ] . mesh . dispose ( true , object . type === "robot" ) ; // Ghost material is shared, don't dispose
442+ lines . shift ( ) ;
443+ }
444+ } else {
445+ // Filter to visible lines
446+ let mechanismLines = state ?. lines . filter (
447+ ( line ) =>
448+ Math . hypot ( line . end [ 1 ] - line . start [ 1 ] , line . end [ 0 ] - line . start [ 0 ] ) >= 1e-3 &&
449+ line . weight * this . MECHANISM_WIDTH_PER_WEIGHT >= 1e-3
483450 ) ;
484- const newTranslation =
485- object . mechanism . axis == "x" ? new THREE . Vector3 ( length / 2 , 0 , 0 ) : new THREE . Vector3 ( 0 , length / 2 , 0 ) ;
486- if ( ! newScale . equals ( meshEntry . scale ) || ! newTranslation . equals ( meshEntry . translation ) ) {
487- meshEntry . geometry . translate ( - meshEntry . translation . x , - meshEntry . translation . y , - meshEntry . translation . z ) ;
488- meshEntry . geometry . scale ( 1 / meshEntry . scale . x , 1 / meshEntry . scale . y , 1 / meshEntry . scale . z ) ;
489- meshEntry . geometry . scale ( newScale . x , newScale . y , newScale . z ) ;
490- meshEntry . geometry . translate ( newTranslation . x , newTranslation . y , newTranslation . z ) ;
491- meshEntry . scale = newScale ;
492- meshEntry . translation = newTranslation ;
451+
452+ // Remove extra lines
453+ while ( lines . length > mechanismLines . length ) {
454+ lines [ 0 ] . mesh . dispose ( true , object . type === "robot" ) ; // Ghost material is shared, don't dispose
455+ lines . shift ( ) ;
493456 }
494457
495- // Update color
496- if ( object . type !== "ghost" ) {
497- meshEntry . material . color = new THREE . Color ( line . color ) ;
458+ // Add new lines
459+ while ( lines . length < mechanismLines . length ) {
460+ const geometry = new THREE . BoxGeometry ( 1 , 1 , 1 ) ;
461+ const material =
462+ object . type === "ghost"
463+ ? this . ghostMaterial
464+ : new THREE . MeshPhongMaterial ( { specular : this . materialSpecular , shininess : this . materialShininess } ) ;
465+ lines . push ( {
466+ mesh : new ResizableInstancedMesh ( this . root , [ { geometry : geometry , material : material } ] ) ,
467+ geometry : geometry ,
468+ scale : new THREE . Vector3 ( 1 , 1 , 1 ) ,
469+ translation : new THREE . Vector3 ( ) ,
470+ material : material
471+ } ) ;
498472 }
499473
500- // Update pose
501- meshEntry . mesh . setPoses (
502- object . poses
503- . map ( ( x ) => x . pose )
504- . map ( ( robotPose ) => {
505- this . dummyRobotPose . rotation . setFromQuaternion ( rotation3dToQuaternion ( robotPose . rotation ) ) ;
506- this . dummyRobotPose . position . set ( ...robotPose . translation ) ;
474+ // Update children
475+ for ( let i = 0 ; i < mechanismLines . length ; i ++ ) {
476+ const line = mechanismLines [ i ] ;
477+ const meshEntry = lines [ i ] ;
478+
479+ const length = Math . hypot ( line . end [ 1 ] - line . start [ 1 ] , line . end [ 0 ] - line . start [ 0 ] ) ;
480+ const angle = Math . atan2 ( line . end [ 1 ] - line . start [ 1 ] , line . end [ 0 ] - line . start [ 0 ] ) ;
481+
482+ // Update length
483+ const newScale = new THREE . Vector3 (
484+ length ,
485+ line . weight * this . MECHANISM_WIDTH_PER_WEIGHT ,
486+ line . weight * this . MECHANISM_WIDTH_PER_WEIGHT
487+ ) ;
488+ const newTranslation = new THREE . Vector3 ( length / 2 , 0 , 0 ) ;
489+ if ( ! newScale . equals ( meshEntry . scale ) || ! newTranslation . equals ( meshEntry . translation ) ) {
490+ meshEntry . geometry . translate ( - meshEntry . translation . x , - meshEntry . translation . y , - meshEntry . translation . z ) ;
491+ meshEntry . geometry . scale ( 1 / meshEntry . scale . x , 1 / meshEntry . scale . y , 1 / meshEntry . scale . z ) ;
492+ meshEntry . geometry . scale ( newScale . x , newScale . y , newScale . z ) ;
493+ meshEntry . geometry . translate ( newTranslation . x , newTranslation . y , newTranslation . z ) ;
494+ meshEntry . scale = newScale ;
495+ meshEntry . translation = newTranslation ;
496+ }
507497
508- if ( object . mechanism ?. axis == "x" ) {
509- this . dummyUserPose . position . set ( line . start [ 0 ] - object . mechanism ! . dimensions [ 0 ] / 2 , 0 , line . start [ 1 ] ) ;
498+ // Update color
499+ if ( object . type !== "ghost" ) {
500+ meshEntry . material . color = new THREE . Color ( line . color ) ;
501+ }
502+
503+ // Update pose
504+ meshEntry . mesh . setPoses (
505+ object . poses
506+ . map ( ( x ) => x . pose )
507+ . map ( ( robotPose ) => {
508+ this . dummyRobotPose . rotation . setFromQuaternion ( rotation3dToQuaternion ( robotPose . rotation ) ) ;
509+ if ( plane == "yz" ) this . dummyRobotPose . rotateZ ( Math . PI / 2 ) ;
510+ this . dummyRobotPose . position . set ( ...robotPose . translation ) ;
511+
512+ this . dummyUserPose . position . set ( line . start [ 0 ] - state . dimensions [ 0 ] / 2 , 0 , line . start [ 1 ] ) ;
510513 this . dummyUserPose . rotation . set ( 0 , - angle , 0 ) ;
511- } else {
512- this . dummyUserPose . position . set ( 0 , line . start [ 0 ] - object . mechanism ! . dimensions [ 0 ] / 2 , line . start [ 1 ] ) ;
513- this . dummyUserPose . rotation . set ( angle , 0 , 0 ) ;
514- }
515- return {
516- translation : this . dummyUserPose . getWorldPosition ( new THREE . Vector3 ( ) ) . toArray ( ) ,
517- rotation : quaternionToRotation3d ( this . dummyUserPose . getWorldQuaternion ( new THREE . Quaternion ( ) ) )
518- } ;
519- } )
520- ) ;
514+ return {
515+ translation : this . dummyUserPose . getWorldPosition ( new THREE . Vector3 ( ) ) . toArray ( ) ,
516+ rotation : quaternionToRotation3d ( this . dummyUserPose . getWorldQuaternion ( new THREE . Quaternion ( ) ) )
517+ } ;
518+ } )
519+ ) ;
520+ }
521521 }
522- }
522+ } ;
523+ updateMechanism ( object . mechanisms . xz , this . mechanismLinesXZ , "xz" ) ;
524+ updateMechanism ( object . mechanisms . yz , this . mechanismLinesYZ , "yz" ) ;
523525
524526 // Update swerve canvas (disabled in XR)
525527 if ( ! this . isXR ) {
0 commit comments